Worlds drive practice

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

@@ -461,29 +461,30 @@ private void configureOperatorBindings() {
 
     new Trigger(() -> xboxController.getPOV() != -1).onTrue(new EjectAlgaeCommand(algaeSubsystem));
 
-    // Move biscuit
-    new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogBiscuitCommand(
-                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
-        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
-    new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogBiscuitCommand(
-                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
-        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
-
-    // Move elevator
-    new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogElevatorCommand(
-                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
-        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
-    new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogElevatorCommand(
-                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
-        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
+    // // Move biscuit
+    // new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
+    //     .onTrue(
+    //         new JogBiscuitCommand(
+    //             biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
+    //     .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+    // new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
+    //     .onTrue(
+    //         new JogBiscuitCommand(
+    //             biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
+    //     .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+
+    // // Move elevator
+    // new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
+    //     .onTrue(
+    //         new JogElevatorCommand(
+    //             elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
+    //     .onFalse(new HoldElevatorCommand(elevatorSubsystem));
+    // new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
+    //     .onTrue(
+    //         new JogElevatorCommand(
+    //             elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown,
+    // Rotations)))
+    //     .onFalse(new HoldElevatorCommand(elevatorSubsystem));
   }
 
   private void configureTestOperatorBindings() {

src/main/java/frc/robot/commands/auton/DriveAlgaeWaitAutonServoCommand.java

@@ -4,10 +4,12 @@
 import choreo.trajectory.SwerveSample;
 import choreo.trajectory.Trajectory;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.commands.drive.DriveAutonCommand;
 import frc.robot.constants.AutonConstants;
+import frc.robot.constants.DriveConstants;
 import frc.robot.constants.PathHandlerConstants;
 import frc.robot.subsystems.biscuit.BiscuitSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
@@ -155,7 +157,13 @@ public void execute() {
         desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
         driveSubsystem.calculateController(desiredState);
 
-        if (shouldTransitionToServoing()) {
+        double currX = driveSubsystem.getPoseMeters().getX();
+
+        if (shouldTransitionToServoing()
+            && (currX > DriveConstants.kCenterLineX
+                    && robotStateSubsystem.getAllianceColor() == Alliance.Blue
+                || robotStateSubsystem.getAllianceColor() == Alliance.Red
+                    && currX < DriveConstants.kCenterLineX)) {
           isServoing = true;
 
           visionSubsystem.setIsAuto(true);

src/main/java/frc/robot/commands/auton/NonProcessorSlowMicCommand.java

@@ -0,0 +1,136 @@
+package frc.robot.commands.auton;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.drive.PrepOdomForAutoCommand;
+import frc.robot.commands.elevator.ZeroElevatorCommand;
+import frc.robot.commands.pathHandler.StartPathHandlerCommand;
+import frc.robot.constants.PathHandlerConstants;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.pathHandler.PathHandler;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
+import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
+import frc.robot.subsystems.vision.VisionSubsystem;
+import java.util.ArrayList;
+import java.util.List;
+
+public class NonProcessorSlowMicCommand extends SequentialCommandGroup
+    implements AutoCommandInterface {
+  private PathHandler pathHandler;
+  private DriveSubsystem driveSubsystem;
+  private DriveAutonServoCommand startPath;
+  private CoralSubsystem coralSubsystem;
+  private RobotStateSubsystem robotStateSubsystem;
+  private VisionSubsystem visionSubsystem;
+  private List<Character> NodeNames;
+  private List<ScoringLevel> NodeLevels;
+  private List<Double> lightDistances = new ArrayList<>();
+  private List<Double> postOffsets = new ArrayList<>();
+  private char startNode;
+
+  public NonProcessorSlowMicCommand(
+      DriveSubsystem driveSubsystem,
+      PathHandler pathHandler,
+      RobotStateSubsystem robotStateSubsystem,
+      AlgaeSubsystem algaeSubsystem,
+      BiscuitSubsystem biscuitSubsystem,
+      CoralSubsystem coralSubsystem,
+      ElevatorSubsystem elevatorSubsystem,
+      TagAlignSubsystem tagAlignSubsystem,
+      VisionSubsystem visionSubsystem,
+      String startPathName,
+      List<Character> NodeNames,
+      List<ScoringLevel> NodeLevels,
+      List<Double> lightDistances,
+      List<Double> postOffsets,
+      char startNode,
+      Boolean startScoreLeft,
+      Pose2d startPose) {
+    addRequirements(
+        driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
+    this.pathHandler = pathHandler;
+    this.driveSubsystem = driveSubsystem;
+    this.coralSubsystem = coralSubsystem;
+    this.robotStateSubsystem = robotStateSubsystem;
+    this.visionSubsystem = visionSubsystem;
+
+    this.NodeNames = NodeNames;
+    this.NodeLevels = NodeLevels;
+    this.startNode = startNode;
+    this.lightDistances = lightDistances;
+    this.postOffsets = postOffsets;
+
+    startPath =
+        new DriveAutonServoCommand(
+            driveSubsystem,
+            tagAlignSubsystem,
+            elevatorSubsystem,
+            biscuitSubsystem,
+            robotStateSubsystem,
+            startPathName,
+            true,
+            true,
+            false,
+            startScoreLeft,
+            postOffsets.get(0));
+    postOffsets.remove(0);
+
+    addCommands(
+        new SequentialCommandGroup(
+            new ParallelCommandGroup(
+                new PrepOdomForAutoCommand(
+                    robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(180.0), startPose),
+                new ZeroElevatorCommand(elevatorSubsystem)),
+            // new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
+            startPath,
+            new PlaceCoralAutonCommand(robotStateSubsystem, coralSubsystem),
+
+            // new ParallelCommandGroup(
+            //     new ZeroElevatorCommand(elevatorSubsystem),
+            // new ResetOdometryCommand(
+            // driveSubsystem, new Pose2d(7.1008875, 5.0756788, new Rotation2d(180))),
+            // new SequentialCommandGroup(
+            //     new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
+            //     new ResetOdometryCommand(driveSubsystem, AutonConstants.kNonProcessorShallow))),
+            // new ParallelCommandGroup(
+            // //     new WaitCommand(0.03), new SetVisionUpdatesCommand(visionSubsystem, true)),
+            // startPath,
+            // new WaitForButtonPressCommand(button),
+            new StartPathHandlerCommand(
+                pathHandler,
+                PathHandlerConstants.kSlowShallowPathNames,
+                NodeNames,
+                NodeLevels,
+                startNode,
+                false)));
+  }
+
+  @Override
+  public void reassignAlliance() {
+    startPath.reassignAlliance();
+    driveSubsystem.teleResetGyro();
+    coralSubsystem.setAutoPreload();
+    robotStateSubsystem.setIsAutoPlacing(false);
+    robotStateSubsystem.setScoringLevel(ScoringLevel.L4);
+    robotStateSubsystem.setGetAlgaeOnCycle(false);
+    // robotStateSubsystem.setIsAuto(true);
+    robotStateSubsystem.setScoreSide(ScoreSide.RIGHT);
+    visionSubsystem.setVisionUpdating(true);
+    pathHandler.setPathNames(PathHandlerConstants.kSlowShallowPathNames);
+    pathHandler.setNodeNames(NodeNames);
+    pathHandler.setNodeLevels(NodeLevels);
+    pathHandler.setStartNode(startNode);
+    pathHandler.setGetAlgaeLast(false);
+    pathHandler.setLightDistances(lightDistances);
+    pathHandler.setPostOffsets(postOffsets);
+    // pathHandler.reassignAlliance();
+  }
+}

src/main/java/frc/robot/commands/auton/StealOneAlgeaNoSuperCycleAutonCommand.java

@@ -96,6 +96,7 @@ public StealOneAlgeaNoSuperCycleAutonCommand(
                 robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(180.0), startPose),
             // new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
             firstPath,
+            new PlaceCoralAutonCommand(robotStateSubsystem, coralSubsystem),
             new WaitForElevBelowBarge(elevatorSubsystem),
             secondPath,
             thirdPath,

src/main/java/frc/robot/commands/auton/StealTwoAlgeaNoSuperCycleAutonCommand.java

@@ -0,0 +1,161 @@
+package frc.robot.commands.auton;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.drive.PrepOdomForAutoCommand;
+import frc.robot.commands.robotState.AutoScoreAlgaeCommand;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
+import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
+import frc.robot.subsystems.vision.VisionSubsystem;
+
+public class StealTwoAlgeaNoSuperCycleAutonCommand extends SequentialCommandGroup
+    implements AutoCommandInterface {
+
+  private DriveSubsystem driveSubsystem;
+  private DriveAutonServoCommand firstPath;
+  private DriveAlgaeWaitAutonServoCommand secondPath;
+  private DriveBargeAutonCommand thirdPath;
+  private DriveAlgaeWaitAutonServoCommand fourthPath;
+  private DriveBargeAutonCommand fifthPath;
+  private CoralSubsystem coralSubsystem;
+  private RobotStateSubsystem robotStateSubsystem;
+  private VisionSubsystem visionSubsystem;
+
+  public StealTwoAlgeaNoSuperCycleAutonCommand(
+      DriveSubsystem driveSubsystem,
+      RobotStateSubsystem robotStateSubsystem,
+      AlgaeSubsystem algaeSubsystem,
+      BiscuitSubsystem biscuitSubsystem,
+      CoralSubsystem coralSubsystem,
+      ElevatorSubsystem elevatorSubsystem,
+      TagAlignSubsystem tagAlignSubsystem,
+      VisionSubsystem visionSubsystem,
+      String firstPathName,
+      String secondPathName,
+      String thirdPathName,
+      String fourthPathName,
+      String fifthPathName,
+      ScoringLevel OppAlgeaHeight1,
+      ScoringLevel OppAlgeaHeight2,
+      Pose2d startPose) {
+    addRequirements(
+        driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
+    this.driveSubsystem = driveSubsystem;
+    this.coralSubsystem = coralSubsystem;
+    this.robotStateSubsystem = robotStateSubsystem;
+    this.visionSubsystem = visionSubsystem;
+
+    firstPath =
+        new DriveAutonServoCommand(
+            driveSubsystem,
+            tagAlignSubsystem,
+            elevatorSubsystem,
+            biscuitSubsystem,
+            robotStateSubsystem,
+            firstPathName,
+            true,
+            true,
+            false,
+            false,
+            0.0);
+
+    secondPath =
+        new DriveAlgaeWaitAutonServoCommand(
+            driveSubsystem,
+            tagAlignSubsystem,
+            elevatorSubsystem,
+            biscuitSubsystem,
+            robotStateSubsystem,
+            visionSubsystem,
+            secondPathName,
+            false,
+            true,
+            false,
+            0.0,
+            OppAlgeaHeight1,
+            1.5);
+
+    thirdPath =
+        new DriveBargeAutonCommand(
+            driveSubsystem,
+            tagAlignSubsystem,
+            elevatorSubsystem,
+            biscuitSubsystem,
+            robotStateSubsystem,
+            visionSubsystem,
+            thirdPathName,
+            true,
+            false);
+
+    fourthPath =
+        new DriveAlgaeWaitAutonServoCommand(
+            driveSubsystem,
+            tagAlignSubsystem,
+            elevatorSubsystem,
+            biscuitSubsystem,
+            robotStateSubsystem,
+            visionSubsystem,
+            fourthPathName,
+            false,
+            true,
+            false,
+            0.0,
+            OppAlgeaHeight2,
+            0.0);
+
+    fifthPath =
+        new DriveBargeAutonCommand(
+            driveSubsystem,
+            tagAlignSubsystem,
+            elevatorSubsystem,
+            biscuitSubsystem,
+            robotStateSubsystem,
+            visionSubsystem,
+            fifthPathName,
+            true,
+            false);
+
+    addCommands(
+        new SequentialCommandGroup(
+            new PrepOdomForAutoCommand(
+                robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(180.0), startPose),
+            // new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
+            firstPath,
+            new PlaceCoralAutonCommand(robotStateSubsystem, coralSubsystem),
+            new WaitForElevBelowBarge(elevatorSubsystem),
+            secondPath,
+            thirdPath,
+            new AutoScoreAlgaeCommand(
+                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem),
+            fourthPath,
+            fifthPath,
+            new AutoScoreAlgaeCommand(
+                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)));
+  }
+
+  @Override
+  public void reassignAlliance() {
+    firstPath.reassignAlliance();
+    secondPath.reassignAlliance();
+    thirdPath.reassignAlliance();
+    fourthPath.reassignAlliance();
+    fifthPath.reassignAlliance();
+    driveSubsystem.teleResetGyro();
+    coralSubsystem.setAutoPreload();
+    robotStateSubsystem.setIsAutoPlacing(false);
+    robotStateSubsystem.setScoringLevel(ScoringLevel.L4);
+    robotStateSubsystem.setGetAlgaeOnCycle(false);
+    // robotStateSubsystem.setIsAuto(true);
+    robotStateSubsystem.setScoreSide(ScoreSide.RIGHT);
+    visionSubsystem.setVisionUpdating(true);
+    // pathHandler.reassignAlliance();
+  }
+}