Use commands to run autos

autobuilder-robot/build.gradle

@@ -10,7 +10,8 @@ sourceCompatibility = JavaVersion.VERSION_17
 targetCompatibility = JavaVersion.VERSION_17
 
 group = 'com.dacubeking'
-version = '2.3.0'
+
+version = '2.3.0-command-rewrite-beta'
 var wpilibVersion = "2024.1.1"
 
 repositories {

autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/GuiAuto.java

@@ -10,34 +10,99 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;
 
 import java.io.File;
 import java.io.IOException;
 import java.util.ArrayList;
+import java.util.Collections;
+import java.util.LinkedList;
 import java.util.List;
+import java.util.concurrent.Callable;
 import java.util.concurrent.ExecutionException;
 
-import static com.dacubeking.AutoBuilder.robot.robotinterface.AutonomousContainer.getCommandTranslator;
+public class GuiAuto extends CommandBase {
 
-public class GuiAuto implements Runnable {
+
+    @Override
+    public void execute() {
+        if (autonomous == DO_NOTHING_AUTONOMOUS_FAILED_LOADING) {
+            DriverStation.reportError("Failed to load autonomous. See logs for more details.", false);
+        }
+
+        if (isFirstRun) {
+            if (initialPose != null) {
+                AutonomousContainer.getInstance().getInitialPoseSetter().accept(initialPose);
+                AutonomousContainer.getInstance().printDebug("Set initial pose: " + initialPose);
+            } else {
+                AutonomousContainer.getInstance().printDebug("No initial pose set");
+            }
+
+            if (!abstractAutonomousSteps.isEmpty()) {
+                abstractAutonomousSteps.peek().initialize();
+            }
+
+            isFirstRun = false;
+        }
+
+        if (abstractAutonomousSteps.isEmpty()) {
+            return;
+        }
+
+        AbstractAutonomousStep autonomousStep = abstractAutonomousSteps.peek();
+
+        try {
+            if (autonomousStep.execute(scriptsToExecuteByTime, scriptsToExecuteByPercent)) {
+                autonomousStep.end();
+                abstractAutonomousSteps.remove();
+                if (!abstractAutonomousSteps.isEmpty()) {
+                    abstractAutonomousSteps.peek().initialize();
+                }
+
+                //Sort the lists to make sure they are sorted by time
+                Collections.sort(scriptsToExecuteByTime);
+                Collections.sort(scriptsToExecuteByPercent);
+            }
+        } catch (CommandExecutionFailedException | ExecutionException e) {
+            DriverStation.reportError("Failed to execute autonomous step. " + e.getMessage(), e.getStackTrace());
+            abstractAutonomousSteps.clear(); // Will end the auto
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        if (!abstractAutonomousSteps.isEmpty()) {
+            abstractAutonomousSteps.peek().end();
+        }
+
+        abstractAutonomousSteps.clear();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return abstractAutonomousSteps.isEmpty();
+    }
 
     private static final Autonomous DO_NOTHING_AUTONOMOUS = new Autonomous(new ArrayList<>());
+    private static final Autonomous DO_NOTHING_AUTONOMOUS_FAILED_LOADING = new Autonomous(new ArrayList<>());
+
     private @NotNull Autonomous autonomous = DO_NOTHING_AUTONOMOUS; // default to do nothing in case of some error
     private @Nullable Pose2d initialPose;
 
+    private final Callable<Autonomous> autonomousSupplier;
+
     /**
      * Ensure you are creating the objects for your auto on robot init. The roborio will take multiple seconds to initialize the
      * auto.
      *
      * @param autonomousFile File location of the auto
      */
-    public GuiAuto(File autonomousFile) throws IOException {
-        autonomous = (Autonomous) Serializer.deserializeFromFile(autonomousFile, Autonomous.class,
+    public GuiAuto(File autonomousFile) {
+        autonomousSupplier = () -> (Autonomous) Serializer.deserializeFromFile(autonomousFile, Autonomous.class,
                 autonomousFile.getName().endsWith(".json"));
-        init();
+        this.setName(autonomousFile.getPath() + "GuiAuto");
     }
 
     /**
@@ -47,92 +112,56 @@ public GuiAuto(File autonomousFile) throws IOException {
      * @param autonomousJson String of the autonomous
      */
     public GuiAuto(String autonomousJson) {
-        try {
-            autonomous = (Autonomous) Serializer.deserialize(autonomousJson, Autonomous.class, true);
-        } catch (IOException e) {
-            DriverStation.reportError("Failed to deserialize auto. " + e.getMessage(), e.getStackTrace());
-            // The do nothing auto will be used
-        }
-        init();
+        autonomousSupplier = () -> (Autonomous) Serializer.deserialize(autonomousJson, Autonomous.class, true);
+        this.setName("JsonGuiAuto");
     }
 
-    /**
-     * Finds and saves the initial pose of the robot.
-     */
-    private void init() {
-        for (AbstractAutonomousStep autonomousStep : autonomous.getAutonomousSteps()) {
-            if (autonomousStep instanceof TrajectoryAutonomousStep) {
-                TrajectoryAutonomousStep trajectoryAutonomousStep = (TrajectoryAutonomousStep) autonomousStep;
-                Trajectory.State initialState = trajectoryAutonomousStep.getTrajectory().getStates().get(0);
-                initialPose = new Pose2d(initialState.poseMeters.getTranslation(),
-                        trajectoryAutonomousStep.getRotations().get(0).getRotation());
-                break;
-            }
-        }
-    }
 
     /**
-     * Runs the autonomous.
+     * Loads the autonomous and saves the initial pose.
      */
-    @Override
-    public void run() {
-        AutonomousContainer.getInstance().isInitialized();
-
-        Thread.currentThread().setUncaughtExceptionHandler((t, e) -> {
-            DriverStation.reportError("Uncaught exception in auto thread: " + e.getMessage(), e.getStackTrace());
-            getCommandTranslator().stopRobot();
-        });
-
-        if (autonomous == DO_NOTHING_AUTONOMOUS) {
-            DriverStation.reportError("No auto was loaded. Doing nothing.", false);
-            return;
-        }
-
-        AutonomousContainer.getInstance().printDebug("Started Running: " + Timer.getFPGATimestamp());
-
-
-        //Set our initial pose in our robot tracker
-        if (initialPose != null) {
-            getCommandTranslator().setRobotPose(initialPose);
-            AutonomousContainer.getInstance().printDebug("Set initial pose: " + initialPose);
-        } else {
-            AutonomousContainer.getInstance().printDebug("No initial pose set");
-        }
-
-        //Loop though all the steps and execute them
-        List<SendableScript> scriptsToExecuteByTime = new ArrayList<>();
-        List<SendableScript> scriptsToExecuteByPercent = new ArrayList<>();
-
-        for (AbstractAutonomousStep autonomousStep : autonomous.getAutonomousSteps()) {
-            AutonomousContainer.getInstance().printDebug("Doing a step: " + Timer.getFPGATimestamp());
-
-            if (Thread.interrupted()) {
-                getCommandTranslator().stopRobot();
-                AutonomousContainer.getInstance().printDebug("Auto was interrupted " + Timer.getFPGATimestamp());
-                return;
+    public void loadAutonomous() throws IOException {
+        try {
+            if (autonomousSupplier == null) {
+                throw new IllegalStateException("Autonomous supplier is null");
             }
 
-            try {
-                autonomousStep.execute(scriptsToExecuteByTime, scriptsToExecuteByPercent);
-            } catch (InterruptedException e) {
-                getCommandTranslator().stopRobot();
-                AutonomousContainer.getInstance().printDebug("Auto prematurely stopped at " + Timer.getFPGATimestamp() +
-                        ". This is not an error if you disabled your robot.");
-                if (AutonomousContainer.getInstance().areDebugPrintsEnabled()) {
-                    e.printStackTrace();
+            if (autonomous == DO_NOTHING_AUTONOMOUS) {
+                AutonomousContainer.getInstance().printDebug("Loading autonomous: " + this.getName());
+                autonomous = autonomousSupplier.call();
+                AutonomousContainer.getInstance().printDebug("Loaded autonomous: " + this.getName());
+
+                for (AbstractAutonomousStep autonomousStep : autonomous.getAutonomousSteps()) {
+                    if (autonomousStep instanceof TrajectoryAutonomousStep trajectoryAutonomousStep) {
+                        Trajectory.State initialState = trajectoryAutonomousStep.getTrajectory().getStates().get(0);
+                        initialPose = new Pose2d(initialState.poseMeters.getTranslation(),
+                                trajectoryAutonomousStep.getRotations().get(0).getRotation());
+                        break;
+                    }
                 }
-                return;
-            } catch (CommandExecutionFailedException | ExecutionException e) {
-                getCommandTranslator().stopRobot();
-                e.printStackTrace(); // We should always print this out since it is a fatal error
-                return;
             }
+        } catch (IOException e) {
+            autonomous = DO_NOTHING_AUTONOMOUS_FAILED_LOADING;
+            throw e;
+        } catch (Exception e) {
+            throw new RuntimeException(e);
         }
+    }
 
-        System.out.println("Finished Autonomous at " + Timer.getFPGATimestamp());
-        getCommandTranslator().stopRobot();
+    @Override
+    public void initialize() {
+        scriptsToExecuteByPercent = new ArrayList<>();
+        scriptsToExecuteByTime = new ArrayList<>();
+        abstractAutonomousSteps = new LinkedList<>(autonomous.getAutonomousSteps());
+        isFirstRun = true;
     }
 
+    private List<SendableScript> scriptsToExecuteByTime;
+    private List<SendableScript> scriptsToExecuteByPercent;
+    private LinkedList<AbstractAutonomousStep> abstractAutonomousSteps;
+
+    private boolean isFirstRun = true;
+
     /**
      * Gets the initial pose of the robot.
      *

autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/NetworkAuto.java

@@ -5,6 +5,8 @@
 import edu.wpi.first.networktables.NetworkTableInstance;
 import org.jetbrains.annotations.NotNull;
 
+import java.io.IOException;
+
 public class NetworkAuto extends GuiAuto {
 
     static final @NotNull NetworkTableInstance instance = NetworkTableInstance.getDefault();
@@ -14,7 +16,8 @@ public class NetworkAuto extends GuiAuto {
     /**
      * Deserializes an auto form a NT entry.
      */
-    public NetworkAuto() {
+    public NetworkAuto() throws IOException {
         super(autoPath.getString(null));
+        loadAutonomous();
     }
 }

autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/annotations/AutoBuilderRobotSide.java

@@ -0,0 +1,14 @@
+package com.dacubeking.AutoBuilder.robot.annotations;
+
+import java.lang.annotation.ElementType;
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.lang.annotation.Target;
+
+/**
+ * Annotation that marks that this field/method is only used when running on the robot
+ */
+@Retention(RetentionPolicy.SOURCE)
+@Target({ElementType.METHOD, ElementType.FIELD, ElementType.TYPE, ElementType.CONSTRUCTOR, ElementType.PARAMETER})
+public @interface AutoBuilderRobotSide {
+}

autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/command/AutoBuilderSwerveControllerCommand.java

@@ -0,0 +1,105 @@
+package com.dacubeking.AutoBuilder.robot.command;
+
+import com.dacubeking.AutoBuilder.robot.robotinterface.TrajectoryBuilderInfo;
+import edu.wpi.first.math.controller.HolonomicDriveController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
+
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * {@link SwerveControllerCommand}, but adds a check for position error before finishing a path, and removes constructors the
+ * imply the heading from the trajectory. (as the {@link TrajectoryBuilderInfo#desiredHeadingSupplier()} should be passed to the
+ * desiredRotation
+ */
+public class AutoBuilderSwerveControllerCommand extends SwerveControllerCommand {
+    /**
+     * Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will not
+     * return output voltages but rather raw module states from the position controllers which need to be put into a velocity
+     * PID.
+     *
+     * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path.
+     * This is left to the user to do since it is not appropriate for paths with nonstationary endstates.
+     *
+     * @param trajectory         The trajectory to follow.
+     * @param pose               A function that supplies the robot pose - use one of the odometry classes to provide this.
+     * @param kinematics         The kinematics for the robot drivetrain.
+     * @param xController        The Trajectory Tracker PID controller for the robot's x position.
+     * @param yController        The Trajectory Tracker PID controller for the robot's y position.
+     * @param thetaController    The Trajectory Tracker PID controller for angle for the robot.
+     * @param tolerance          tolerance for the HolonomicDriveController. Will prevent the path from completing if the robot is
+     *                           not within the defined tolerance. see: {@link HolonomicDriveController#setTolerance(Pose2d)}
+     * @param desiredRotation    The angle that the drivetrain should be facing. This is sampled at each time step.
+     * @param outputModuleStates The raw output module states from the position controllers.
+     * @param requirements       The subsystems to require.
+     */
+    public AutoBuilderSwerveControllerCommand(Trajectory trajectory,
+                                              Supplier<Pose2d> pose,
+                                              SwerveDriveKinematics kinematics,
+                                              PIDController xController,
+                                              PIDController yController,
+                                              ProfiledPIDController thetaController,
+                                              Pose2d tolerance,
+                                              Supplier<Rotation2d> desiredRotation,
+                                              Consumer<SwerveModuleState[]> outputModuleStates,
+                                              Subsystem... requirements) {
+
+        this(
+                trajectory,
+                pose,
+                kinematics,
+                new HolonomicDriveController(
+                        requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
+                        requireNonNullParam(yController, "yController", "SwerveControllerCommand"),
+                        requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand")),
+                tolerance,
+                desiredRotation,
+                outputModuleStates,
+                requirements);
+    }
+
+    /**
+     * Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will not
+     * return output voltages but rather raw module states from the position controllers which need to be put into a velocity
+     * PID.
+     *
+     * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
+     * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+     *
+     * @param trajectory         The trajectory to follow.
+     * @param pose               A function that supplies the robot pose - use one of the odometry classes to provide this.
+     * @param kinematics         The kinematics for the robot drivetrain.
+     * @param controller         The HolonomicDriveController for the drivetrain.
+     * @param tolerance          tolerance for the HolonomicDriveController. Will prevent the path from completing if the robot is
+     *                           not within the defined tolerance. see: {@link HolonomicDriveController#setTolerance(Pose2d)}
+     * @param desiredRotation    The angle that the drivetrain should be facing. This is sampled at each time step.
+     * @param outputModuleStates The raw output module states from the position controllers.
+     * @param requirements       The subsystems to require.
+     */
+    public AutoBuilderSwerveControllerCommand(Trajectory trajectory, Supplier<Pose2d> pose, SwerveDriveKinematics kinematics,
+                                              HolonomicDriveController controller,
+                                              Pose2d tolerance, Supplier<Rotation2d> desiredRotation,
+                                              Consumer<SwerveModuleState[]> outputModuleStates, Subsystem... requirements) {
+        super(trajectory, pose, kinematics, controller, desiredRotation, outputModuleStates, requirements);
+        this.controller = controller;
+        controller.setTolerance(tolerance);
+    }
+
+    private final HolonomicDriveController controller;
+
+    @Override
+    public boolean isFinished() {
+        assert controller != null;
+        return controller.atReference();
+    }
+}