make less dynamic, but actually shuttles now

diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java
index ae84a9a973fa53b41c23dbf584dee963348ee791..3e8aafe3e577da533ea93dffad7d2e05db3f1398 100644 (file)
--- a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java
+++ b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java
@@ -6,8 +6,10 @@ import choreo.auto.AutoTrajectory;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.DoNothing;
 import frc.robot.commands.gpm.IntakeMovementCommand;
+import frc.robot.commands.gpm.RunSpindexer;
 import frc.robot.commands.gpm.RunSpindexerWithStop;
 import frc.robot.subsystems.Intake.Intake;
 import frc.robot.subsystems.hood.Hood;
@@ -280,11 +282,13 @@ public class ChoreoPathCommandBuilder {
         }),
         shuttlingTrajectory.cmd()));
 
+    routine.active().whileTrue(new RunSpindexer(spindexer, turret, hood, intake));
+
     shuttlingTrajectory.done().onTrue(Commands.sequence(
         new InstantCommand(() -> {
           hood.forceHoodDown(false);
         }),
-        new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+        new WaitCommand(2.0).raceWith(new IntakeMovementCommand(intake)),
         new InstantCommand(() -> {
           intake.extend();
           intake.spinStart();
@@ -298,9 +302,9 @@ public class ChoreoPathCommandBuilder {
 
   public AutoRoutine rightSuperShuttling(AutoFactory factory) {
     AutoRoutine routine = factory.newRoutine("rightSuperShuttling");
-    
-    AutoTrajectory shuttlingTrajectory = routine.trajectory("superShuttling").mirrorY();
-    
+
+    AutoTrajectory shuttlingTrajectory = routine.trajectory("superShuttling");
+
     routine.active().onTrue(Commands.sequence(
         shuttlingTrajectory.resetOdometry(),
         new InstantCommand(() -> {
@@ -309,11 +313,13 @@ public class ChoreoPathCommandBuilder {
         }),
         shuttlingTrajectory.cmd()));
 
+    routine.active().whileTrue(new RunSpindexer(spindexer, turret, hood, intake));
+
     shuttlingTrajectory.done().onTrue(Commands.sequence(
         new InstantCommand(() -> {
           hood.forceHoodDown(false);
         }),
-        new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+        new WaitCommand(2.0).raceWith(new IntakeMovementCommand(intake)),
         new InstantCommand(() -> {
           intake.extend();
           intake.spinStart();