auto factories

src/main/java/frc/robot/Robot.java

@@ -1,7 +1,13 @@
 package frc.robot;
 
-import edu.wpi.first.wpilibj2.command.Command;
+import choreo.auto.AutoChooser;
+import choreo.auto.AutoFactory;
+import choreo.auto.AutoRoutine;
+import choreo.auto.AutoTrajectory;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import frc.robot.extras.simulation.field.SimulatedField;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
@@ -17,8 +23,8 @@
  * project.
  */
 public class Robot extends LoggedRobot {
-  private Command m_autonomousCommand;
-  private RobotContainer m_robotContainer;
+  private RobotContainer robotContainer;
+
 
   public Robot() {
     // Record metadata
@@ -42,34 +48,33 @@
     }
 
     // Set up data receivers & replay source
     switch (Constants.CURRENT_MODE) {
       case COMP_ROBOT:
         // Running on a real robot, log to a USB stick ("/U/logs")
         Logger.addDataReceiver(new WPILOGWriter());
         // Gets data from network tables
         Logger.addDataReceiver(new NT4Publisher());
         break;
 
       case SIM_ROBOT:
         // Running a physics simulator, log to NT
         Logger.addDataReceiver(new NT4Publisher());
         break;
 
       case DEV_ROBOT:
         // Replaying a log, set up replay source
         setUseTiming(false); // Run as fast as possible
         String logPath = LogFileUtil.findReplayLog();
         Logger.setReplaySource(new WPILOGReader(logPath));
         Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
         break;
     }
 
     // Start AdvantageKit logger
     Logger.start();
 
     // Instantiate our RobotContainer. This will perform all our button bindings,
-    // and put our autonomous chooser on the dashboard.
-    m_robotContainer = new RobotContainer();
+    robotContainer = new RobotContainer();
   }
 
   /** This function is called periodically during all modes. */
@@ -98,33 +103,19 @@
   @Override
   public void disabledPeriodic() {}
 
-  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
-  @Override
-  public void autonomousInit() {
-    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
-    // schedule the autonomous command (example)
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.schedule();
-    }
-  }
-
   /** This function is called periodically during autonomous. */
   @Override
   public void autonomousPeriodic() {}
+  @Override
+  public void autonomousInit() {
+    autoChooser.selectedCommandScheduler();
 
+  }
   /** This function is called once when teleop is enabled. */
   @Override
   public void teleopInit() {
-    // This makes sure that the autonomous stops running when
-    // teleop starts running. If you want the autonomous to
-    // continue until interrupted by another command, remove
-    // this line or comment it out.
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
-    }
-
-    m_robotContainer.teleopInit();
+    robotContainer.teleopInit();
+
   }
 
   /** This function is called periodically during operator control. */
@@ -150,6 +141,6 @@
   @Override
   public void simulationPeriodic() {
     SimulatedField.getInstance().simulationPeriodic();
-    m_robotContainer.updateFieldSimAndDisplay();
+    robotContainer.updateFieldSimAndDisplay();
   }
 }

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

@@ -1,10 +1,11 @@
 package frc.robot;
 
+import choreo.auto.AutoChooser;
+import choreo.auto.AutoFactory;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -17,6 +18,8 @@
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP;
 import frc.robot.extras.util.JoystickUtil;
+import frc.robot.extras.util.AllianceFlipper;
+import frc.robot.subsystems.example.ExampleSubsystem;
 import frc.robot.subsystems.swerve.SwerveConstants;
 import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
 import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
@@ -46,13 +49,21 @@ public class RobotContainer {
   private final SwerveDriveSimulation swerveDriveSimulation;
   private final GyroSimulation gyroSimulation;
 
-  private final SendableChooser<Command> autoChooser;
+  private AutoFactory autoFactory;
+  public AutoChooser autoChooser;
 
   public RobotContainer() {
-    autoChooser = new SendableChooser<Command>();
-    autoChooser.setDefaultOption("Auto", null);
+    // this will put our autonomous chooser on the dashboard.
+    autoChooser = new AutoChooser();
+    autoChooser.addRoutine("Example routine", this::exampleAutoRoutine);
+    SmartDashboard.putData(autoChooser);
 
     switch (Constants.CURRENT_MODE) {
+       // this will put our autonomous chooser on the dashboard.
+      autoChooser = new AutoChooser();
+      autoChooser.addRoutine("Example routine", this::exampleAutoRoutine);
+      SmartDashboard.putData(autoChooser);
+
       case COMP_ROBOT -> {
         /* Real robot, instantiate hardware IO implementations */
 
@@ -69,12 +80,36 @@ public RobotContainer() {
                 new CompModule(SwerveConstants.moduleConfigs[2]),
                 new CompModule(SwerveConstants.moduleConfigs[3]));
         visionSubsystem = new VisionSubsystem(new PhysicalVision());
+        exampleSubsystem = new ExampleSubsystem();
+
+         // this sets up the auto factory
+    autoFactory =
+        new AutoFactory(
+            swerveDrive::getEstimatedPose, // A function that returns the current robot pose
+            swerveDrive::resetEstimatedPose, // A function that resets the current robot pose to the
+            // provided Pose2d
+            swerveDrive::followTrajectory, // The drive subsystem trajectory follower
+            AllianceFlipper.isRed(), // If alliance flipping should be enabled
+            swerveDrive); // The drive subsystem
+
       }
       case DEV_ROBOT -> {
         swerveDrive = new SwerveDrive(null, null, null, null, null);
         gyroSimulation = null;
         swerveDriveSimulation = null;
         visionSubsystem = null;
+        exampleSubsystem = new ExampleSubsystem();
+
+         // this sets up the auto factory
+    autoFactory =
+        new AutoFactory(
+            swerveDrive::getEstimatedPose, // A function that returns the current robot pose
+            swerveDrive::resetEstimatedPose, // A function that resets the current robot pose to the
+            // provided Pose2d
+            swerveDrive::followTrajectory, // The drive subsystem trajectory follower
+            AllianceFlipper.isRed(), // If alliance flipping should be enabled
+            swerveDrive); // The drive subsystem
+
       }
 
       case SIM_ROBOT -> {
@@ -113,6 +148,18 @@ public RobotContainer() {
                 new SimulatedModule(swerveDriveSimulation.getModules()[3]));
 
         visionSubsystem = null;
+        exampleSubsystem = new ExampleSubsystem();
+
+         // this sets up the auto factory
+    autoFactory =
+        new AutoFactory(
+            swerveDrive::getEstimatedPose, // A function that returns the current robot pose
+            swerveDrive::resetEstimatedPose, // A function that resets the current robot pose to the
+            // provided Pose2d
+            swerveDrive::followTrajectory, // The drive subsystem trajectory follower
+            AllianceFlipper.isRed(), // If alliance flipping should be enabled
+            swerveDrive); // The drive subsystem
+
         //     new VisionSubsystem(
         //         new SimulatedVision(() -> swerveDriveSimulation.getSimulatedDriveTrainPose()));
 
@@ -136,10 +183,56 @@ public RobotContainer() {
                 new ModuleInterface() {},
                 new ModuleInterface() {},
                 new ModuleInterface() {});
+        exampleSubsystem = new ExampleSubsystem();
+
+         // this sets up the auto factory
+    autoFactory =
+        new AutoFactory(
+            swerveDrive::getEstimatedPose, // A function that returns the current robot pose
+            swerveDrive::resetEstimatedPose, // A function that resets the current robot pose to the
+            // provided Pose2d
+            swerveDrive::followTrajectory, // The drive subsystem trajectory follower
+            AllianceFlipper.isRed(), // If alliance flipping should be enabled
+            swerveDrive); // The drive subsystem
+
       }
     }
   }
 
+  public AutoRoutine exampleAutoRoutine() {
+
+    AutoRoutine routine = autoFactory.newRoutine("exampleAutoRoutine");
+
+    AutoTrajectory startToETraj = routine.trajectory("startToE");
+    AutoTrajectory eToPickupTraj = routine.trajectory("eToPickup");
+    AutoTrajectory cToPickupTraj = routine.trajectory("cToPickup");
+    AutoTrajectory pickupToCTraj = routine.trajectory("pickupToC");
+
+    // reset odometry and start first trajectory
+    routine.active().onTrue(Commands.sequence(startToETraj.resetOdometry(), startToETraj.cmd()));
+
+    startToETraj
+        .active()
+        .onTrue(
+            exampleSubsystem
+                .exampleFunctionalCommand()); // TODO: replace with elevator to L4 command
+    startToETraj
+        .atTime("score")
+        .onTrue(
+            exampleSubsystem.exampleFunctionalCommand()); // TODO: replace with command for rollers
+    startToETraj
+        .done()
+        .onTrue(
+            eToPickupTraj
+                .cmd()
+                .alongWith(
+                    exampleSubsystem
+                        .exampleFunctionalCommand())); // TODO: replace with elevator to intake
+    // command
+
+    return routine;
+  }
+
   private void resetFieldAndOdometryForAuto(Pose2d robotStartingPoseAtBlueAlliance) {
     final Pose2d startingPose = robotStartingPoseAtBlueAlliance;
 
@@ -215,8 +308,7 @@ public Command getAutonomousCommand() {
         new Pose2d(
             swerveDrive.getEstimatedPose().getX(),
             swerveDrive.getEstimatedPose().getY(),
-            Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset())));
-    return autoChooser.getSelected();
+            Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset())));  
   }
 
   public void updateFieldSimAndDisplay() {

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

@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.example.ExampleSubsystem;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ExampleAuto extends SequentialCommandGroup {
+
+  /** Creates a new ExampleAuto. */
+  public ExampleAuto(ExampleSubsystem exampleSubsystem) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands(exampleSubsystem.setPivotToScore().alongWith(exampleSubsystem.setPivotToScore()));
+  }
+}

src/main/java/frc/robot/commands/example/ExampleScoreL4Command.java

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.example;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class ExampleScoreL4Command extends Command {
+  /** Creates a new ExampleScoreL4Command. */
+  public ExampleScoreL4Command() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/subsystems/example/ExampleConstants.java

@@ -0,0 +1,5 @@
+package frc.robot.subsystems.example;
+
+public class ExampleConstants {
+  public static final double SHOOTER_PIVOT_ANGLE = 0 - 9;
+}