auto factories

src/main/java/frc/robot/Autos.java

@@ -0,0 +1,47 @@
+package frc.robot;
+
+import choreo.auto.AutoFactory;
+import choreo.auto.AutoRoutine;
+import choreo.auto.AutoTrajectory;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.example.ExampleSubsystem;
+import frc.robot.subsystems.swerve.SwerveDrive;
+
+public class Autos {
+  public AutoFactory autoFactory;
+
+  public AutoRoutine exampleAutoRoutine(
+      ExampleSubsystem exampleSubsystem, SwerveDrive swerveDrive) {
+
+    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 with vision and start first trajectory
+    routine.active().onTrue(Commands.sequence(swerveDrive.getEstimatedPose(), 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;
+  }
+}

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

@@ -1,6 +1,5 @@
 package frc.robot;
 
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.extras.simulation.field.SimulatedField;
 import org.littletonrobotics.junction.LogFileUtil;
@@ -17,8 +16,7 @@
  * project.
  */
 public class Robot extends LoggedRobot {
-  private Command m_autonomousCommand;
-  private RobotContainer m_robotContainer;
+  private RobotContainer robotContainer;
 
   public Robot() {
     // Record metadata
@@ -42,34 +40,33 @@
     }
 
     // Set up data receivers & replay source
     switch (Constants.ROBOT_TYPE) {
       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 +95,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() {
+    robotContainer.getAutonomousCommand();
+  }
+
   /** 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 +133,6 @@
   @Override
   public void simulationPeriodic() {
     SimulatedField.getInstance().simulationPeriodic();
-    m_robotContainer.updateFieldSimAndDisplay();
+    robotContainer.updateFieldSimAndDisplay();
   }
 }

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

@@ -1,22 +1,28 @@
 package frc.robot;
 
+import choreo.auto.AutoChooser;
+import choreo.auto.AutoFactory;
+import choreo.trajectory.SwerveSample;
 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.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.SimulationConstants;
 import frc.robot.commands.drive.DriveCommand;
+import frc.robot.commands.drive.FollowChoreoTrajectory;
 import frc.robot.extras.simulation.field.SimulatedField;
 import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP;
+import frc.robot.extras.util.AllianceFlipper;
 import frc.robot.extras.util.JoystickUtil;
+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;
@@ -37,25 +43,24 @@
 
   private final VisionSubsystem visionSubsystem;
   private final SwerveDrive swerveDrive;
-
+  public final ExampleSubsystem exampleSubsystem;
   private final CommandXboxController operatorController = new CommandXboxController(1);
   private final CommandXboxController driverController = new CommandXboxController(0);
 
   // Simulation, we store them here in the robot container
   // private final SimulatedField simulatedArena;
   private final SwerveDriveSimulation swerveDriveSimulation;
   private final GyroSimulation gyroSimulation;
 
-  private final SendableChooser<Command> autoChooser;
+  public AutoFactory autoFactory;
+  public AutoChooser autoChooser;
+  public Autos autos;
 
   public RobotContainer() {
-    autoChooser = new SendableChooser<Command>();
-    autoChooser.setDefaultOption("Auto", null);
 
     switch (Constants.ROBOT_TYPE) {
       case COMP_ROBOT -> {
         /* Real robot, instantiate hardware IO implementations */
-
         /* Disable Simulations */
         // this.simulatedArena = null;
         this.gyroSimulation = null;
@@ -136,6 +141,30 @@
                 new ModuleInterface() {});
       }
     }
+
+    exampleSubsystem = new ExampleSubsystem();
+
+    autos = new Autos();
+    // this adds an auto routine to the auto chooser
+    autoChooser.addRoutine(
+        "Example routine", () -> autos.exampleAutoRoutine(exampleSubsystem, swerveDrive));
+
+    // this updates the auto chooser
+    SmartDashboard.putData(autoChooser);
+
+    // 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
+            (SwerveSample sample) -> {
+              FollowChoreoTrajectory command =
+                  new FollowChoreoTrajectory(swerveDrive, visionSubsystem, sample);
+              command.execute();
+            }, // The drive subsystem trajectory follower
+            AllianceFlipper.isRed(), // If alliance flipping should be enabled
+            swerveDrive); // The drive subsystem
   }
 
   private void resetFieldAndOdometryForAuto(Pose2d robotStartingPoseAtBlueAlliance) {
@@ -215,7 +244,7 @@
             swerveDrive.getEstimatedPose().getX(),
             swerveDrive.getEstimatedPose().getY(),
             Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset())));
-    return autoChooser.getSelected();
+    return autoChooser.selectedCommand();
   }
 
   public void updateFieldSimAndDisplay() {

src/main/java/frc/robot/commands/drive/FollowChoreoTrajectory.java

@@ -0,0 +1,42 @@
+package frc.robot.commands.drive;
+
+import choreo.trajectory.SwerveSample;
+import frc.robot.subsystems.swerve.SwerveDrive;
+import frc.robot.subsystems.vision.VisionSubsystem;
+
+public class FollowChoreoTrajectory extends DriveCommandBase {
+  private final SwerveDrive swerveDrive;
+  private SwerveSample sample;
+
+  // Constructor without TrajectorySample for AutoFactory
+  public FollowChoreoTrajectory(
+      SwerveDrive swerveDrive, VisionSubsystem vision, SwerveSample sample) {
+    super(swerveDrive, vision);
+    this.swerveDrive = swerveDrive;
+    this.sample = sample;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(swerveDrive, vision);
+  }
+
+  // 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() {
+    if (sample != null) {
+      swerveDrive.followTrajectory(sample);
+    }
+  }
+
+  // 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;
+}

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

@@ -0,0 +1,102 @@
+// 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.subsystems.example;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import java.util.function.DoubleSupplier;
+
+public class ExampleSubsystem extends SubsystemBase {
+  private DoubleSupplier DOUBLELOL;
+
+  /** Creates a new ExampleSubsystem. */
+  public ExampleSubsystem() {}
+
+  // this is the skeleton for the subsystem
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+
+  private void exampleSubsystemFunction() {}
+
+  private void setExamplePivotAngle(double angle) {
+    // normal subsystem stuff
+  }
+
+  private double getExamplePivotAngle() {
+    return 0.0;
+  }
+
+  private boolean isAngleSet() {
+    return false;
+  }
+
+  private boolean isGamepieceinRobot() {
+    return false;
+  }
+
+  // Public Command Factory
+  // create these for everything you would need to access from a command
+  // so you don't input values or acces the internals of a subsystem in the commands themselves
+  public Command exampleFunctionalCommand() {
+    // the command is written here instead of in it's own seperate file.
+    // this saves space and makes the code more readable
+
+    return new FunctionalCommand(
+        // does on init
+        () -> this.setExamplePivotAngle(0),
+        // does on execute
+        () -> this.setExamplePivotAngle(1),
+        // does when command ends
+        interrupted -> this.setExamplePivotAngle(0),
+        // ends the command when this is true
+        () -> getExamplePivotAngle() >= 0,
+        // requirements for the command
+        this);
+  }
+
+  // Shared internal implementation
+  private Command exampleStartEndCommand() {
+    // the command is written here instead of in it's own seperate file.
+    // this saves space and makes the code more readable
+
+    return new StartEndCommand(
+        // Sets the pivot to 1 while the command is active
+        () -> this.setExamplePivotAngle(1),
+        // Sets the pivot to 0 when the command ends
+        () -> this.setExamplePivotAngle(0),
+        // requirements for the command
+        this);
+  }
+
+  // Shared internal implementation
+  private Command exampleRunOnceCommand(double angle) {
+    return this.runOnce(() -> setExamplePivotAngle(angle));
+  }
+
+  // Public Command Factory
+  public Command setPivotToScore() {
+    return exampleRunOnceCommand(ExampleConstants.SHOOTER_PIVOT_ANGLE);
+  }
+
+  // Public Command Factory
+  public Command setPivotToAngle(DoubleSupplier angle) {
+    // if you need to pass a value in do it this way
+    return exampleRunOnceCommand(angle.getAsDouble());
+  }
+
+  // this is a trigger
+  // if you need to give information from the subsystem do it like this
+  // must be a boolean
+  public final Trigger isAngleInRightSpot = new Trigger(() -> isAngleSet());
+
+  // another trigger example
+  public final Trigger hasGamepiece = new Trigger(() -> isGamepieceinRobot());
+}

src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java

@@ -145,8 +145,10 @@ public static final class TrajectoryConstants {
     public static final double MAX_ACCELERATION = 3;
 
     public static final double AUTO_TRANSLATION_P = 1.5; // 1.7
+    public static final double AUTO_TRANSLATION_I = 0;
     public static final double AUTO_TRANSLATION_D = 0.2;
     public static final double AUTO_THETA_P = 4.5; // 5
+    public static final double AUTO_THETA_I = 0; // 5
     public static final double AUTO_THETA_D = 0.4;
 
     public static final double AUTO_SHOOT_HEADING_OFFSET = 2;