add intake shake command

src/main/deploy/choreo/Rho.chor

@@ -66,64 +66,64 @@
  "config":{
   "frontLeft":{
    "x":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"11.25 in",
+    "val":0.28575
    },
    "y":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"9.75 in",
+    "val":0.24765
    }
   },
   "backLeft":{
    "x":{
-    "exp":"-11 in",
-    "val":-0.2794
+    "exp":"-11.25 in",
+    "val":-0.28575
    },
    "y":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"9.75 in",
+    "val":0.24765
    }
   },
   "mass":{
-   "exp":"125 lbs",
+   "exp":"130 lbs",
    "val":56.69904625
   },
   "inertia":{
    "exp":"6 kg m ^ 2",
    "val":6.0
   },
   "gearing":{
-   "exp":"6.5",
-   "val":6.5
+   "exp":"6.75",
+   "val":6.75
   },
   "radius":{
    "exp":"2 in",
    "val":0.0508
   },
   "vmax":{
-   "exp":"6000 RPM",
-   "val":628.3185307179587
+   "exp":"3500 RPM",
+   "val":366.5191429188092
   },
   "tmax":{
-   "exp":"1.2 N * m",
-   "val":1.2
+   "exp":"0.25 N * m",
+   "val":0.25
   },
   "cof":{
-   "exp":"1.5",
-   "val":1.5
+   "exp":"1",
+   "val":1.0
   },
   "bumper":{
    "front":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"17.938 in",
+    "val":0.4556251999999999
    },
    "side":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"16.438 in",
+    "val":0.41752519999999993
    },
    "back":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"17.938 in",
+    "val":0.4556251999999999
    }
   },
   "differentialTrackWidth":{

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

@@ -772,13 +772,9 @@ public void bindXboxOperator(int port, DriverController driver) {
     // xboxOperator.y().and(() -> hopper.isDeployed()).whileTrue(intake.getReverseCommand());
 
     // Feeder
-    xboxOperator
-        .a()
-        .whileTrue(Commands.startEnd(feeder::spinForward, () -> {}, feeder).withName("Advance"));
+    xboxOperator.a().whileTrue(feeder.getSpinForwardCommand());
 
-    xboxOperator
-        .x()
-        .whileTrue(Commands.startEnd(feeder::reverse, () -> {}, feeder).withName("Reverse"));
+    xboxOperator.x().whileTrue(feeder.getReverseCommand());
 
     // Desaturate turret and advance feeder
     xboxOperator.rightBumper().whileTrue(createDesaturateAndShootCommand(driver));
@@ -835,8 +831,7 @@ private Command createDesaturateAndShootCommand(DriverController driver) {
                 allianceSelector::fieldRotated)
             .withName("Desaturate turret"),
         Commands.sequence(
-            Commands.waitUntil(launcher::isTurretDesaturated),
-            Commands.startEnd(feeder::spinForward, () -> {}, feeder).withName("Advance")));
+            Commands.waitUntil(launcher::isTurretDesaturated), feeder.getSpinForwardCommand()));
   }
 
   private static void logHIDs() {

src/main/java/frc/robot/auto/AutoMode.java

@@ -5,13 +5,24 @@
 import choreo.auto.AutoTrajectory;
 import choreo.trajectory.SwerveSample;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.feeder.Feeder;
+import frc.robot.subsystems.intake.Intake;
 import java.util.Optional;
 
 public abstract class AutoMode {
   private final AutoFactory autoFactory;
-
-  public AutoMode(Drive drivetrain) {
+  protected final Drive drive;
+  protected final Feeder feeder;
+  protected final Intake intake;
+
+  public AutoMode(Drive drivetrain, Feeder feeder, Intake intake) {
+    this.drive = drivetrain;
+    this.feeder = feeder;
+    this.intake = intake;
     autoFactory =
         new AutoFactory(
             drivetrain::getPose,
@@ -34,10 +45,18 @@ public AutoFactory getAutoFactory() {
   public Optional<Pose2d> getInitialPose() {
     return getInitialTrajectory().getInitialPose();
   }
-  ;
 
   public SwerveSample[] getLoggableTrajectory() {
     SwerveSample[] trajArray = new SwerveSample[0];
     return getInitialTrajectory().getRawTrajectory().samples().toArray(trajArray);
   }
+
+  protected Command stopDrive() {
+    return DriveCommands.getStopCommand(drive);
+  }
+
+  protected Command shakeAndFeed(double timeoutSeconds) {
+    return Commands.parallel(feeder.getSpinForwardCommand(), intake.getShakeIntakeCommand())
+        .withTimeout(timeoutSeconds);
+  }
 }

src/main/java/frc/robot/auto/B_LeftTrenchAuto.java

@@ -10,33 +10,23 @@
 import frc.robot.subsystems.launcher.Launcher;
 
 public class B_LeftTrenchAuto extends AutoMode {
-  Drive drive;
-  // Hopper hopper;
-  Feeder feeder;
-  Intake intake;
-  Launcher launcher;
+  private final Launcher launcher;
+  private final AutoRoutine routine;
+  private final AutoTrajectory blueLeftNeutralZone;
+  private final AutoTrajectory blueLeftTransitionToNZ;
 
   public B_LeftTrenchAuto(
       Drive drivetrain,
-      // Hopper hopperSubsystem,
       Feeder feederSubsystem,
       Intake intakeSubsystem,
       Launcher launcherSubsystem) {
-    super(drivetrain);
-    drive = drivetrain;
-    // hopper = hopperSubsystem;
-    feeder = feederSubsystem;
-    intake = intakeSubsystem;
+    super(drivetrain, feederSubsystem, intakeSubsystem);
     launcher = launcherSubsystem;
+    routine = getAutoFactory().newRoutine("B_LeftTrenchAuto");
+    blueLeftNeutralZone = routine.trajectory("BlueLeftNeutralZone");
+    blueLeftTransitionToNZ = routine.trajectory("BlueLeftTransitionToNZ");
   }
 
-  // Define routine
-  AutoRoutine routine = super.getAutoFactory().newRoutine("B_LeftTrenchAuto");
-
-  // Load trajectories
-  AutoTrajectory blueLeftNeutralZone = routine.trajectory("BlueLeftNeutralZone");
-  AutoTrajectory blueLeftTransitionToNZ = routine.trajectory("BlueLeftTransitionToNZ");
-
   @Override
   public String getName() {
     return "BlueLeftTrenchAuto";
@@ -57,28 +47,20 @@ public AutoRoutine getAutoRoutine() {
                 blueLeftNeutralZone.resetOdometry(),
                 DriveCommands.getChassisAimingCommand(drive, launcher::getTurretDesaturationDelta)
                     .withTimeout(1.5),
-                Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(3.0),
-                Commands.runOnce(feeder::stop, feeder),
+                feeder.getSpinForwardCommand().withTimeout(3.0),
                 Commands.parallel(
                     blueLeftNeutralZone.cmd(), intake.getDeployCommand().withTimeout(10.0))));
 
     blueLeftNeutralZone
         .done()
         .onTrue(
             Commands.sequence(
-                Commands.runOnce(drive::stop, drive),
-                Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
-                Commands.runOnce(feeder::stop, feeder),
+                stopDrive(),
+                shakeAndFeed(5.0),
                 Commands.parallel(
                     blueLeftTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0))));
 
-    blueLeftTransitionToNZ
-        .done()
-        .onTrue(
-            Commands.sequence(
-                Commands.runOnce(drive::stop, drive),
-                Commands.startEnd(feeder::spinForward, () -> {}, feeder).withTimeout(5.0),
-                Commands.runOnce(feeder::stop, feeder)));
+    blueLeftTransitionToNZ.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0)));
 
     return routine;
   }