Example implementation of a choreo auto

src/main/deploy/choreo/path.chor

@@ -0,0 +1,84 @@
+{
+ "name":"path",
+ "version":2,
+ "type":"Swerve",
+ "variables":{
+  "expressions":{},
+  "poses":{}
+ },
+ "config":{
+  "frontLeft":{
+   "x":{
+    "exp":"11 in",
+    "val":0.2794
+   },
+   "y":{
+    "exp":"11 in",
+    "val":0.2794
+   }
+  },
+  "backLeft":{
+   "x":{
+    "exp":"-11 in",
+    "val":-0.2794
+   },
+   "y":{
+    "exp":"11 in",
+    "val":0.2794
+   }
+  },
+  "mass":{
+   "exp":"150 lbs",
+   "val":68.0388555
+  },
+  "inertia":{
+   "exp":"6 kg m ^ 2",
+   "val":6.0
+  },
+  "gearing":{
+   "exp":"6.5",
+   "val":6.5
+  },
+  "radius":{
+   "exp":"2 in",
+   "val":0.0508
+  },
+  "vmax":{
+   "exp":"6000 RPM",
+   "val":628.3185307179587
+  },
+  "tmax":{
+   "exp":"1.2 N * m",
+   "val":1.2
+  },
+  "cof":{
+   "exp":"1.5",
+   "val":1.5
+  },
+  "bumper":{
+   "front":{
+    "exp":"16 in",
+    "val":0.4064
+   },
+   "side":{
+    "exp":"16 in",
+    "val":0.4064
+   },
+   "back":{
+    "exp":"16 in",
+    "val":0.4064
+   }
+  },
+  "differentialTrackWidth":{
+   "exp":"22 in",
+   "val":0.5588
+  }
+ },
+ "generationFeatures":[],
+ "codegen":{
+  "root":"Users/michael/Developer/FRC2026_Java/src/main/deploy/choreo",
+  "genVars":true,
+  "genTrajData":true,
+  "useChoreoLib":true
+ }
+}

src/main/java/frc/robot/Drive.java

@@ -0,0 +1,34 @@
+package frc.robot;
+
+import choreo.trajectory.SwerveSample;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+
+
+public class Drive extends SubsystemBase{
+    private final PIDController xController = new PIDController(.01, 0.0, 0.0);
+    private final PIDController yController = new PIDController(.01, 0.0, 0.0);
+    private final PIDController headingController = new PIDController(.01, 0.0, 0.0);
+    private SwerveSubsystem subsystem;
+
+    public Drive(SwerveSubsystem subsystem) {
+        headingController.enableContinuousInput(-Math.PI, Math.PI);
+        this.subsystem = subsystem;
+    }
+
+    public void followTrajectory(SwerveSample sample) {
+        Pose2d pose = subsystem.getPose();
+
+        ChassisSpeeds speeds = new ChassisSpeeds(
+            sample.vx + xController.calculate(pose.getX(), sample.x),
+            sample.vy + yController.calculate(pose.getX(), sample.y),
+            sample.omega + headingController.calculate(pose.getRotation().getRadians(), sample.heading)
+            );
+
+            subsystem.drive(speeds);
+
+    }
+}

src/main/java/frc/robot/RobotContainer.java

@@ -28,6 +28,7 @@
 import frc.robot.autochooser.AutoChooser;
 import frc.robot.commands.angler.AimAngler;
 import frc.robot.commands.angler.RunAnglerToReverseLimit;
+import frc.robot.commands.auto.ExampleAuto;
 import frc.robot.commands.shooter.SetShootingState;
 import frc.robot.commands.turret.RunTurretToFwdLimit;
 import frc.robot.commands.turret.RunTurretToRevLimit;
@@ -64,6 +65,14 @@
 
 import java.io.File;
 
+import choreo.auto.AutoFactory;
+import choreo.auto.AutoRoutine;
+import choreo.auto.AutoTrajectory;
+
+import choreo.auto.AutoFactory;
+import choreo.auto.AutoRoutine;
+import choreo.auto.AutoTrajectory;
+
 /**
  * This class is where the bulk of the robot should be declared. Since
  * Command-based is a
@@ -78,21 +87,25 @@ public class RobotContainer {
         private final AutoChooser autoChooser = new AutoChooser();
         // The robot's subsystems and commands are defined here...
         // private final TiltSubsystem tiltSubsystem;
-        private final ClimberSubsystem climberSubsystem;
         private final AnglerSubsystem anglerSubsystem;
         private final IntakeSubsystem intakeSubsystem;
         private final FeederSubsystem feederSubsystem;
         private final ApriltagSubsystem apriltagSubsystem;
         private final ShooterSubsystem shooterSubsystem;
         private RobotVisualizer robotVisualizer = null;
         private final HopperSubsystem hopperSubsystem;
+        private final ClimberSubsystem climberSubsystem;
     private final TurretSubsystem turretSubsystem;
         private final IntakeDeployerSubsystem intakeDeployer;
         private SwerveSubsystem drivebase = null;
         private GyroSubsystem gyroSubsystem = null;
         private final CommandJoystick driveJoystick = new CommandJoystick(Constants.DRIVE_JOYSTICK_PORT);
         private final CommandJoystick steerJoystick = new CommandJoystick(Constants.STEER_JOYSTICK_PORT);
         private ShootingState shootState = new ShootingState(ShootState.STOPPED);
+        private Drive drive;
+        private AutoFactory autoFactory;
+        private static AutoRoutine straightRoutine;
+        private static AutoTrajectory straightTrajectory;
 
         // Replace with CommandPS4Controller or CommandJoystick if needed
         // new CommandXboxController(OperatorConstants.kDriverControllerPort);private
@@ -162,7 +175,8 @@ public RobotContainer() {
                                 turretSubsystem = new TurretSubsystem(TurretSubsystem.createSimIo(robotVisualizer));
                 apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createSimIo());
                                 shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer));
-                                intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createSimIo(robotVisualizer));
+                                intakeDeployer = new IntakeDeployerSubsystem(
+                                                IntakeDeployerSubsystem.createSimIo(robotVisualizer));
 
                                 // No GyroSubsystem in REPLAY for now
                                 // create the drive subsystem with null gyro (use default json)
@@ -177,6 +191,7 @@ public RobotContainer() {
 
                 configureBindings();
                 putShuffleboardCommands();
+                setUpAutoFactory();
         }
 
         /**
@@ -193,6 +208,68 @@ public RobotContainer() {
          * {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
          * joysticks}.
          */
+        private void setUpAutoFactory() {
+
+                drive = new Drive(drivebase);
+
+                // Sets up Choreo with pose, odometry, drivebase, and a follow trajectory
+                // command
+                autoFactory = new AutoFactory(drivebase::getPose,
+                                drivebase::resetOdometry,
+                                drive::followTrajectory,
+                                true,
+                                drivebase);
+
+                // example implementation of autoRoutine
+                if (false) {
+                        // Uses autofactory to create a new routine
+                        straightRoutine = autoFactory.newRoutine("StraightRoutine");
+
+                        /*
+                         * Loads a trajectory created in Choreo given the name
+                         * Can load multiple trajectories from the same routine
+                         * 
+                         * i.e.
+                         * AutoRoutine routine = autoFactory.newRoutine("grabAndScore");
+                         * AutoTrajectory grabTraj = routine.trajectory("grabPiece");
+                         * AutoTrajectory scoreTraj = routine.trajectory("scorePiece");
+                         */
+                        straightTrajectory = straightRoutine.trajectory("StraightPath");
+
+                        /*
+                         * .active() is a trigger that becomes true when the routine is running
+                         * .onTrue() starts a command when the trigger becomes true (i.e. when the
+                         * routine starts)
+                         * 
+                         * Use commands.sequence() to sequence multiple commands (i.e. reset odometry,
+                         * then follow trajectory)
+                         */
+                        straightRoutine.active().onTrue(
+                                        straightTrajectory.resetOdometry()
+                                                        .andThen(straightTrajectory.cmd()));
+
+                        /*
+                         * -----------------------------------------------------------------------------
+                         * -------------------
+                         * Trajectory Triggers (read more on docs page
+                         * https://choreo.autos/choreolib/auto-factory/):
+                         * -----------------------------------------------------------------------------
+                         * -------------------
+                         * 
+                         * trajectory.atTime(String)
+                         * trajectory.atTime(double time)
+                         * trajectory.done()
+                         * trajectory.active()
+                         * trajectory.inactive()
+                         * trajectory.atPose(String, double, double)
+                         * trajectory.atPose(Pose2d, double, double)
+                         * trajectory.doneDelayed(int)
+                         * trajectory.doneFor(int)
+                         * trajectory.recentlyDone()
+                         */
+                }
+        }
+
         private void configureBindings() {
                 // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
                 // new Trigger(m_exampleSubsystem::exampleCondition)
@@ -401,12 +478,14 @@ public void putShuffleboardCommands() {
          * @return the command to run in autonomous
          */
         public Command getAutonomousCommand() {
-                return autoChooser.getCommand();
+                // return autoChooser.getCommand();
+                // return straightRoutine.cmd(straightTrajectory.done());
+                return new ExampleAuto(drivebase, autoFactory);
         }
 
-  public ClimberSubsystem getClimberSubsystem() {
-    return climberSubsystem;
-  }
+        public ClimberSubsystem getClimberSubsystem() {
+                return climberSubsystem;
+        }
 
         public RobotVisualizer getRobotVisualizer() {
                 return robotVisualizer;

src/main/java/frc/robot/commands/PrintCommand.java

@@ -0,0 +1,36 @@
+package frc.robot.commands;
+
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+/**
+ * A command that prints a string when initialized.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class PrintCommand extends LoggableCommand {
+  private final String message;
+
+  /**
+   * Creates a new a PrintCommand.
+   *
+   * @param message the message to print
+   */
+  public PrintCommand(String message) {
+    this.message = message;
+  }
+
+  @Override
+  public void initialize() {
+    System.out.println(message);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}

src/main/java/frc/robot/commands/auto/ExampleAuto.java

@@ -0,0 +1,27 @@
+package frc.robot.commands.auto;
+
+import choreo.auto.AutoFactory;
+import frc.robot.commands.PrintCommand;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommandWrapper;
+import frc.robot.utils.logging.commands.LoggableParallelCommandGroup;
+import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup;
+
+public class ExampleAuto extends LoggableSequentialCommandGroup{
+    public ExampleAuto(SwerveSubsystem subsystem, AutoFactory auto) {
+        super(
+                LoggableCommandWrapper.wrap(auto.resetOdometry("ExamplePathOne")),
+                new LoggableParallelCommandGroup(
+                        LoggableCommandWrapper.wrap((auto.trajectoryCmd("ExamplePathOne"))),
+                        new PrintCommand("Started ExamplePathOne")
+                ),
+                new PrintCommand("Finished ExamplePathOne"),
+
+                new LoggableParallelCommandGroup(
+                        LoggableCommandWrapper.wrap((auto.trajectoryCmd("ExamplePathTwo"))),
+                        new PrintCommand("Started ExamplePathTwo")
+                ),
+                new PrintCommand("Finished ExamplePathTwo")
+        );
+    }
+}

src/main/java/frc/robot/utils/logging/commands/LoggableCommandWrapper.java

@@ -50,4 +50,4 @@ public InterruptionBehavior getInterruptionBehavior() {
   public boolean runsWhenDisabled() {
     return wrap.runsWhenDisabled();
   }
-}
+}

src/main/java/frc/robot/utils/logging/commands/LoggableDeadlineCommandGroup.java

@@ -45,4 +45,4 @@ public LoggableDeadlineCommandGroup withBasicName(String name) {
     basicName = name;
     return this;
   }
-}
+}

src/main/java/frc/robot/utils/logging/commands/LoggableParallelCommandGroup.java

@@ -42,4 +42,4 @@ public LoggableParallelCommandGroup withBasicName(String name) {
     this.basicName = name;
     return this;
   }
-}
+}

src/main/java/frc/robot/utils/logging/commands/LoggableRaceCommandGroup.java

@@ -42,4 +42,4 @@ public LoggableRaceCommandGroup withBasicName(String name) {
     this.basicName = name;
     return this;
   }
-}
+}

src/main/java/frc/robot/utils/logging/commands/LoggableSequentialCommandGroup.java

@@ -41,4 +41,4 @@ public LoggableSequentialCommandGroup withBasicName(String name) {
     this.basicName = name;
     return this;
   }
-}
+}

src/main/java/frc/robot/utils/logging/commands/LoggableWaitCommand.java

@@ -50,4 +50,4 @@ public boolean equals(Object o) {
   public int hashCode() {
     return Objects.hash(basicName, parent);
   }
-}
+}