Auto barge align and 4-piece Auto

src/main/deploy/choreo/2025-project.chor

@@ -232,32 +232,60 @@
    },
    "fetchP":{
     "x":{
-     "exp":"1.69577419757843 m",
-     "val":1.69577419757843
+     "exp":"1.6421304941177368 m",
+     "val":1.6421304941177368
     },
     "y":{
-     "exp":"0.8236 m",
-     "val":0.8236
+     "exp":"0.8500130772590637 m",
+     "val":0.8500130772590637
     },
     "heading":{
      "exp":"54 deg",
      "val":0.942477796076938
     }
    },
-   "start":{
+   "midStart":{
     "x":{
-     "exp":"7.1008875 m",
-     "val":7.1008875
+     "exp":"7.1 m",
+     "val":7.1
+    },
+    "y":{
+     "exp":"6.1663548 m",
+     "val":6.1663548
+    },
+    "heading":{
+     "exp":"3.14159 rad",
+     "val":3.14159
+    }
+   },
+   "midStartP":{
+    "x":{
+     "exp":"7.1 m",
+     "val":7.1
     },
     "y":{
-     "exp":"5.0756788 m",
-     "val":5.0756788
+     "exp":"1.8818 m",
+     "val":1.8818
     },
     "heading":{
      "exp":"180 deg",
      "val":3.141592653589793
     }
    },
+   "start":{
+    "x":{
+     "exp":"7.1 m",
+     "val":7.1
+    },
+    "y":{
+     "exp":"5.07 m",
+     "val":5.07
+    },
+    "heading":{
+     "exp":"3.14159 rad",
+     "val":3.14159
+    }
+   },
    "startDeep":{
     "x":{
      "exp":"7.1008875 m",

src/main/deploy/choreo/ETofetchPSlow.traj

@@ -21,7 +21,7 @@
     {"x":{"exp":"E.x", "val":4.894185251845}, "y":{"exp":"E.y", "val":2.7146580097364}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"4.480600357055664 m", "val":4.480600357055664}, "y":{"exp":"2.3546712398529053 m", "val":2.3546712398529053}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
     {"x":{"exp":"2.1380813121795654 m", "val":2.1380813121795654}, "y":{"exp":"1.227155 m", "val":1.227155}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"fetchP.x", "val":1.6421304941177368}, "y":{"exp":"fetchP.y", "val":0.8500130772590637}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

src/main/deploy/choreo/startToJSlow.traj

@@ -17,8 +17,8 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"5.7971601486206055 m", "val":5.7971601486206055}, "y":{"exp":"5.54304313659668 m", "val":5.54304313659668}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"start.x", "val":7.1}, "y":{"exp":"start.y", "val":5.07}, "heading":{"exp":"start.heading", "val":3.14159}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.7971601486206055 m", "val":5.7971601486206055}, "y":{"exp":"5.54304313659668 m", "val":5.54304313659668}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

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

@@ -82,6 +82,7 @@ public void autonomousInit() {
     if (m_robotContainer.getAutoSwitch().getAutoCommand() != null) {
       m_robotContainer.getAutoSwitch().getAutoCommand().schedule();
     }
+    m_robotContainer.setHeadlights(true);
   }
 
   @Override

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

@@ -6,7 +6,6 @@
 
 import static edu.wpi.first.units.Units.Rotations;
 
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -94,6 +93,7 @@
 import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
 import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
 import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
+import frc.robot.subsystems.tagAlign.BargeAlignSubsystem;
 import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
 import frc.robot.subsystems.vision.VisionSubsystem;
 import java.util.Map;
@@ -133,6 +133,7 @@ public class RobotContainer {
   private final LEDSubsystem ledSubsystem;
 
   private final TagAlignSubsystem tagAlignSubsystem;
+  private final BargeAlignSubsystem bargeAlignSubsystem;
 
   private final VisionSubsystem visionSubsystem;
 
@@ -191,6 +192,7 @@ public RobotContainer() {
     visionSubsystem = new VisionSubsystem(driveSubsystem);
 
     tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem);
+    bargeAlignSubsystem = new BargeAlignSubsystem(flysky, driveSubsystem);
 
     robotStateSubsystem =
         new RobotStateSubsystem(
@@ -205,7 +207,8 @@ public RobotContainer() {
             funnelSubsystem,
             ledSubsystem,
             tagAlignSubsystem,
-            visionSubsystem);
+            visionSubsystem,
+            bargeAlignSubsystem);
 
     driveSubsystem.setRobotStateSubsystem(robotStateSubsystem);
 
@@ -227,11 +230,7 @@ public RobotContainer() {
             visionSubsystem,
             pathHandler);
 
-    testAutonCommand =
-        new TestAutonCommand(
-            driveSubsystem,
-            robotStateSubsystem,
-            new Pose2d(3.85576086490539, 5.073261807735684, Rotation2d.fromDegrees(300.0)));
+    testAutonCommand = new TestAutonCommand(driveSubsystem, robotStateSubsystem, elevatorSubsystem);
 
     configureTelemetry();
     configureDriverBindings();
@@ -728,6 +727,19 @@ public void configTestDash() {
                 driveSubsystem, climbSubsystem, climbAlignSubsystem, robotStateSubsystem))
         .withPosition(0, 1)
         .withSize(1, 1);
+
+    Shuffleboard.getTab("Test")
+        .add(
+            "Headlights On",
+            new InstantCommand(() -> pathHandler.setHeadlights(true)).runsWhenDisabled())
+        .withPosition(2, 2)
+        .withSize(1, 1);
+    Shuffleboard.getTab("Test")
+        .add(
+            "Headlights Off",
+            new InstantCommand(() -> pathHandler.setHeadlights(false)).runsWhenDisabled())
+        .withPosition(3, 2)
+        .withSize(1, 1);
   }
 
   public Command getAutonomousCommand() {
@@ -751,7 +763,7 @@ public void setScoringSide(ScoreSide side) {
   }
 
   public void stow() {
-    robotStateSubsystem.toStow();
+    robotStateSubsystem.toStowSafe();
   }
 
   public AutoSwitch getAutoSwitch() {
@@ -773,4 +785,8 @@ public void finishAuto() {
   public boolean wasScoringCoral() {
     return robotStateSubsystem.getState() == RobotStates.PLACE_CORAL;
   }
+
+  public void setHeadlights(boolean on) {
+    pathHandler.setHeadlights(on);
+  }
 }

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

@@ -29,7 +29,7 @@ public class NonProcessorDeepAutonCommand extends SequentialCommandGroup
   private RobotStateSubsystem robotStateSubsystem;
   private VisionSubsystem visionSubsystem;
   private List<Character> NodeNames;
-  private List<Integer> NodeLevels;
+  private List<ScoringLevel> NodeLevels;
   private char startNode;
   private boolean lastAlgae;
 
@@ -45,7 +45,7 @@ public NonProcessorDeepAutonCommand(
       VisionSubsystem visionSubsystem,
       String startPathName,
       List<Character> NodeNames,
-      List<Integer> NodeLevels,
+      List<ScoringLevel> NodeLevels,
       char startNode,
       boolean startScoreLeft,
       boolean lastAlgae,

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

@@ -29,7 +29,7 @@ public class NonProcessorShallowAutonCommand extends SequentialCommandGroup
   private RobotStateSubsystem robotStateSubsystem;
   private VisionSubsystem visionSubsystem;
   private List<Character> NodeNames;
-  private List<Integer> NodeLevels;
+  private List<ScoringLevel> NodeLevels;
   private char startNode;
   private boolean lastAlgae;
 
@@ -45,7 +45,7 @@ public NonProcessorShallowAutonCommand(
       VisionSubsystem visionSubsystem,
       String startPathName,
       List<Character> NodeNames,
-      List<Integer> NodeLevels,
+      List<ScoringLevel> NodeLevels,
       char startNode,
       boolean startScoreLeft,
       boolean lastAlgae,

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

@@ -30,7 +30,7 @@ public class NonProcessorShallowSlowAutonCommand extends SequentialCommandGroup
   private RobotStateSubsystem robotStateSubsystem;
   private VisionSubsystem visionSubsystem;
   private List<Character> NodeNames;
-  private List<Integer> NodeLevels;
+  private List<ScoringLevel> NodeLevels;
   private char startNode;
 
   public NonProcessorShallowSlowAutonCommand(
@@ -45,7 +45,7 @@ public NonProcessorShallowSlowAutonCommand(
       VisionSubsystem visionSubsystem,
       String startPathName,
       List<Character> NodeNames,
-      List<Integer> NodeLevels,
+      List<ScoringLevel> NodeLevels,
       char startNode,
       Boolean startScoreLeft,
       Pose2d startPose) {

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

@@ -29,7 +29,7 @@ public class ProcessorShallowAutonCommand extends SequentialCommandGroup
   private RobotStateSubsystem robotStateSubsystem;
   private VisionSubsystem visionSubsystem;
   private List<Character> NodeNames;
-  private List<Integer> NodeLevels;
+  private List<ScoringLevel> NodeLevels;
   private char startNode;
   private boolean lastAlgae;
 
@@ -45,7 +45,7 @@ public ProcessorShallowAutonCommand(
       VisionSubsystem visionSubsystem,
       String startPathName,
       List<Character> NodeNames,
-      List<Integer> NodeLevels,
+      List<ScoringLevel> NodeLevels,
       char startNode,
       boolean startScoreLeft,
       boolean lastAlgae,

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

@@ -29,7 +29,7 @@ public class ProcessorShallowSlowAutonCommand extends SequentialCommandGroup
   private RobotStateSubsystem robotStateSubsystem;
   private VisionSubsystem visionSubsystem;
   private List<Character> NodeNames;
-  private List<Integer> NodeLevels;
+  private List<ScoringLevel> NodeLevels;
   private char startNode;
 
   public ProcessorShallowSlowAutonCommand(
@@ -44,7 +44,7 @@ public ProcessorShallowSlowAutonCommand(
       VisionSubsystem visionSubsystem,
       String startPathName,
       List<Character> NodeNames,
-      List<Integer> NodeLevels,
+      List<ScoringLevel> NodeLevels,
       char startNode,
       boolean startScoreLeft,
       Pose2d startPose) {

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

@@ -2,10 +2,13 @@
 
 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.DriveAutonCommand;
 import frc.robot.commands.drive.PrepOdomForAutoCommand;
+import frc.robot.commands.elevator.ZeroElevatorCommand;
 import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.robotState.RobotStateSubsystem;
 
 public class TestAutonCommand extends SequentialCommandGroup implements AutoCommandInterface {
@@ -14,17 +17,21 @@ public class TestAutonCommand extends SequentialCommandGroup implements AutoComm
   private RobotStateSubsystem robotStateSubsystem;
 
   public TestAutonCommand(
-      DriveSubsystem driveSubsystem, RobotStateSubsystem robotStateSubsystem, Pose2d startPose) {
+      DriveSubsystem driveSubsystem,
+      RobotStateSubsystem robotStateSubsystem,
+      ElevatorSubsystem elevatorSubsystem) {
 
     this.driveSubsystem = driveSubsystem;
     this.robotStateSubsystem = robotStateSubsystem;
 
-    this.startPath = new DriveAutonCommand(driveSubsystem, "LTofetch", true, true, false);
+    this.startPath = new DriveAutonCommand(driveSubsystem, "FiveMeterTestPath", true, true, false);
 
     addCommands(
         new SequentialCommandGroup(
-            new PrepOdomForAutoCommand(
-                robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(300.0), startPose),
+            new ParallelCommandGroup(
+                new PrepOdomForAutoCommand(
+                    robotStateSubsystem, driveSubsystem, new Rotation2d(), new Pose2d()),
+                new ZeroElevatorCommand(elevatorSubsystem)),
             // new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
             startPath));
   }

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

@@ -130,7 +130,7 @@ public void initialize() {
       timer.reset();
       // logger.info("Begin Trajectory: {}", trajectoryName);
       desiredState = mirrorToProcessor(trajectory.sampleAt(timer.get(), mirrorTrajectory).get());
-      driveSubsystem.calculateController(desiredState);
+      // driveSubsystem.calculateController(desiredState);
       if (resetOdometry) {
         driveSubsystem.resetOdometry(initialPose);
       }

src/main/java/frc/robot/commands/pathHandler/SetPathHandlerCommand.java

@@ -2,21 +2,22 @@
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.pathHandler.PathHandler;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
 import java.util.List;
 
 public class SetPathHandlerCommand extends InstantCommand {
   private PathHandler pathHandler;
   private String[][] pathNames;
   private List<Character> NodeNames;
-  private List<Integer> NodeLevels;
+  private List<ScoringLevel> NodeLevels;
   private Character startNode;
   private boolean mirrorToProcessor;
 
   public SetPathHandlerCommand(
       PathHandler pathHandler,
       String[][] pathNames,
       List<Character> NodeNames,
-      List<Integer> NodeLevels,
+      List<ScoringLevel> NodeLevels,
       Character startNode,
       boolean mirrorToProcessor) {
     this.pathHandler = pathHandler;

src/main/java/frc/robot/commands/pathHandler/StartPathHandlerCommand.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.pathHandler.PathHandler;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
 import java.util.List;
 
 public class StartPathHandlerCommand extends Command {
@@ -11,7 +12,7 @@ public StartPathHandlerCommand(
       PathHandler pathHandler,
       String[][] pathNames,
       List<Character> NodeNames,
-      List<Integer> NodeLevels,
+      List<ScoringLevel> NodeLevels,
       Character startNode,
       boolean mirrorToProcessor) {
     pathHandler.setPathNames(pathNames);