hopefully fixed pathHandler

PotatoBoyH4 · PotatoBoyH4 · commit 05adf00536a9 · 2025-02-25T20:58:26.000-05:00
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -6,6 +6,8 @@
 
 import static edu.wpi.first.units.Units.Rotations;
 
+import java.util.List;
+
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
@@ -19,6 +21,7 @@
 import frc.robot.commands.algae.OpenLoopAlgaeCommand;
 import frc.robot.commands.algae.ProcessorAlgaeCommand;
 import frc.robot.commands.algae.ToggleHasAlgaeCommand;
+import frc.robot.commands.auton.NonProcessorShallowAutonCommand;
 import frc.robot.commands.biscuit.HoldBiscuitCommand;
 import frc.robot.commands.biscuit.JogBiscuitCommand;
 import frc.robot.commands.coral.EnableEjectBeamCommand;
@@ -46,6 +49,7 @@
 import frc.robot.commands.vision.SetVisionUpdatesCommand;
 import frc.robot.constants.BiscuitConstants;
 import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.PathHandlerConstants;
 import frc.robot.constants.RobotConstants;
 import frc.robot.controllers.FlyskyJoystick;
 import frc.robot.controllers.FlyskyJoystick.Button;
@@ -452,25 +456,24 @@ public void configurePitDashboard() {
         .withPosition(1, 0)
         .withSize(1, 1);
 
-    // Shuffleboard.getTab("Pit")
-    //     .add(
-    //         "Start Auton",
-    //         new NonProcessorShallowAutonCommand(
-    //             driveSubsystem,
-    //             pathHandler,
-    //             robotStateSubsystem,
-    //             algaeSubsystem,
-    //             biscuitSubsystem,
-    //             coralSubsystem,
-    //             elevatorSubsystem,
-    //             tagAlignSubsystem,
-    //             "startToJ",
-    //             PathHandlerConstants.kpathNames,
-    //             List.of('K', 'L', 'M'),
-    //             List.of(4, 4, 4),
-    //             'J'))
-    //     .withPosition(3, 0)
-    //     .withSize(1, 1);
+    Shuffleboard.getTab("Pit")
+        .add(
+            "Start Auton",
+            new NonProcessorShallowAutonCommand(
+                driveSubsystem,
+                pathHandler,
+                robotStateSubsystem,
+                algaeSubsystem,
+                biscuitSubsystem,
+                coralSubsystem,
+                elevatorSubsystem,
+                tagAlignSubsystem,
+                "startToJ",
+                List.of('K', 'L', 'M'),
+                List.of(4, 4, 4),
+                'J'))
+        .withPosition(3, 0)
+        .withSize(1, 1);
   }
 
   public Command getAutonomousCommand() {
diff --git a/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java b/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java
@@ -7,6 +7,7 @@
 import frc.robot.commands.drive.ResetGyroCommand;
 import frc.robot.commands.drive.SetGyroOffsetCommand;
 import frc.robot.commands.elevator.ZeroElevatorCommand;
+import frc.robot.constants.PathHandlerConstants;
 import frc.robot.subsystems.algae.AlgaeSubsystem;
 import frc.robot.subsystems.biscuit.BiscuitSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem;
@@ -33,7 +34,6 @@ public NonProcessorShallowAutonCommand(
       ElevatorSubsystem elevatorSubsystem,
       TagAlignSubsystem tagAlignSubsystem,
       String startPathName,
-      String[][] pathNames,
       List<Character> NodeNames,
       List<Integer> NodeLevels,
       char startNode) {
@@ -52,8 +52,13 @@ public NonProcessorShallowAutonCommand(
                     new ResetGyroCommand(driveSubsystem),
                     new SetGyroOffsetCommand(driveSubsystem, new Rotation2d(180)))),
             startPath,
-            new frc.robot.commands.pathhHandler.StartPathHandlerCommand(
-                pathHandler, pathNames, NodeNames, NodeLevels, startNode, false)));
+            new frc.robot.commands.pathHandler.StartPathHandlerCommand(
+                pathHandler,
+                PathHandlerConstants.kShallowPathNames,
+                NodeNames,
+                NodeLevels,
+                startNode,
+                false)));
   }
 
   public void reassignAlliance() {
diff --git a/src/main/java/frc/robot/commands/pathHandler/KillPathHandlerCommand.java b/src/main/java/frc/robot/commands/pathHandler/KillPathHandlerCommand.java
@@ -1,4 +1,4 @@
-package frc.robot.commands.pathhHandler;
+package frc.robot.commands.pathHandler;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.pathHandler.PathHandler;
diff --git a/src/main/java/frc/robot/commands/pathHandler/SetPathHandlerCommand.java b/src/main/java/frc/robot/commands/pathHandler/SetPathHandlerCommand.java
@@ -1,4 +1,4 @@
-package frc.robot.commands.pathhHandler;
+package frc.robot.commands.pathHandler;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.pathHandler.PathHandler;
diff --git a/src/main/java/frc/robot/commands/pathHandler/StartPathHandlerCommand.java b/src/main/java/frc/robot/commands/pathHandler/StartPathHandlerCommand.java
@@ -1,16 +1,11 @@
-package frc.robot.commands.pathhHandler;
+package frc.robot.commands.pathHandler;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.pathHandler.PathHandler;
 import java.util.List;
 
 public class StartPathHandlerCommand extends Command {
   private PathHandler pathHandler;
-  private String[][] pathNames;
-  private List<Character> NodeNames;
-  private List<Integer> NodeLevels;
-  private Character startNode;
-  private boolean mirrorToProcessor;
 
   public StartPathHandlerCommand(
       PathHandler pathHandler,
@@ -19,12 +14,11 @@ public StartPathHandlerCommand(
       List<Integer> NodeLevels,
       Character startNode,
       boolean mirrorToProcessor) {
-    this.pathHandler = pathHandler;
-    this.pathNames = pathNames;
-    this.NodeNames = NodeNames;
-    this.NodeLevels = NodeLevels;
-    this.startNode = startNode;
-    this.mirrorToProcessor = mirrorToProcessor;
+    pathHandler.setPathNames(pathNames);
+    pathHandler.setNodeNames(NodeNames);
+    pathHandler.setNodeLevels(NodeLevels);
+    pathHandler.setStartNode(startNode);
+    pathHandler.setMirrorToProcessor(mirrorToProcessor);
   }
 
   public StartPathHandlerCommand(PathHandler pathHandler) {
@@ -33,13 +27,6 @@ public StartPathHandlerCommand(PathHandler pathHandler) {
 
   @Override
   public void initialize() {
-    if (pathNames != null) {
-      pathHandler.setPathNames(pathNames);
-      pathHandler.setNodeNames(NodeNames);
-      pathHandler.setNodeLevels(NodeLevels);
-      pathHandler.setStartNode(startNode);
-      pathHandler.setMirrorToProcessor(mirrorToProcessor);
-    }
     pathHandler.startPathHandler();
   }
 
diff --git a/src/main/java/frc/robot/constants/PathHandlerConstants.java b/src/main/java/frc/robot/constants/PathHandlerConstants.java
@@ -1,7 +1,7 @@
 package frc.robot.constants;
 
 public class PathHandlerConstants {
-  public static final String[][] kpathNames = {
+  public static final String[][] kShallowPathNames = {
     {
       "fetchToA",
       "fetchToB",
diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java
@@ -3,8 +3,10 @@
 import choreo.Choreo;
 import choreo.trajectory.SwerveSample;
 import choreo.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.constants.DriveConstants;
+import frc.robot.constants.PathHandlerConstants;
 import frc.robot.constants.RobotStateConstants;
 import frc.robot.constants.TagServoingConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
@@ -24,21 +26,25 @@ public class PathHandler extends MeasurableSubsystem {
   private TagAlignSubsystem tagAlignSubsystem;
   private static final org.slf4j.Logger logger = LoggerFactory.getLogger(PathHandler.class);
 
+  // parameters
+  private String[][] pathNames =
+      PathHandlerConstants.kShallowPathNames; // the array of paths, in string form
+  private List<Character> nodeNames; // the list of nodes, in order, to score on
+  private List<Integer> nodeLevels; // the list of levels, in order, to score on
+  private Character startNode = 'a'; // the node that the robot starts in front of
+  private boolean mirrorToProcessor =
+      false; // whether the robot starts and fetches on the processor side.
+
   private PathStates curState = PathStates.DONE;
   private boolean isHandling = false;
-  private List<Character> nodeNames;
-  private List<Integer> nodeLevels;
   private Timer timer = new Timer();
   private List<Trajectory<SwerveSample>> fetchPaths;
   private List<Trajectory<SwerveSample>> placePaths;
-  private String[][] pathNames = new String[12][2];
   private Trajectory<SwerveSample> currPath;
   private String currPathString;
   private boolean runningPath = false;
   private boolean isServoing = false;
   private boolean mirrorTrajectory = false;
-  private boolean mirrorToProcessor = false;
-  private Character startNode = 'a';
 
   public PathHandler(
       DriveSubsystem driveSubsystem,
@@ -55,8 +61,8 @@ public PathHandler(
       TagAlignSubsystem tagAlignSubsystem,
       RobotStateSubsystem robotStateSubsystem,
       String[][] pathNames,
-      List<Character> NodeNames,
-      List<Integer> NodeLevels,
+      List<Character> nodeNames,
+      List<Integer> nodeLevels,
       Character startNode,
       boolean mirrorToProcessor) {
     this.driveSubsystem = driveSubsystem;
@@ -73,17 +79,18 @@ public void setPathNames(String[][] pathNames) {
     if (!isHandling) {
       this.pathNames = pathNames;
     }
+    reassignAlliance();
   }
 
-  public void setNodeNames(List<Character> NodeNames) {
+  public void setNodeNames(List<Character> nodeNames) {
     if (!isHandling) {
-      this.nodeNames = NodeNames;
+      this.nodeNames = nodeNames;
     }
   }
 
-  public void setNodeLevels(List<Integer> NodeLevels) {
+  public void setNodeLevels(List<Integer> nodeLevels) {
     if (!isHandling) {
-      this.nodeLevels = NodeLevels;
+      this.nodeLevels = nodeLevels;
     }
   }
 
@@ -163,7 +170,7 @@ private void drivePathServo() {
     if (isHandling && runningPath && currPath != null && isServoing) {
       driveSubsystem.calculateControllerServo(
           mirrorToProcessor(currPath.sampleAt(timer.get(), mirrorTrajectory).get()),
-          0.0); // TODO: use tag servoing here when ready
+          tagAlignSubsystem.calculateAlignY());
       if (timer.hasElapsed(currPath.getTotalTime())) {
         driveSubsystem.setAutoDebugMsg("End " + currPathString);
         runningPath = false;
@@ -200,6 +207,8 @@ private Trajectory<SwerveSample> nextPath() {
   private void advanceNodes() {
     if (nodeNames.size() > 0) {
       nodeNames.remove(0);
+      tagAlignSubsystem.setup(
+          mirrorTrajectory ? Alliance.Red : Alliance.Blue, (nodeNames.get(0) - 'a') % 2 == 0);
     }
   }
 

Original file line number	Diff line number	Diff line change
`@@ -1,4 +1,4 @@`
`1`		`-package frc.robot.commands.pathhHandler;`
	`1`	`+package frc.robot.commands.pathHandler;`
`2`	`2`
`3`	`3`	`import edu.wpi.first.wpilibj2.command.InstantCommand;`
`4`	`4`	`import frc.robot.subsystems.pathHandler.PathHandler;`
Original file line number	Diff line number	Diff line change
`@@ -1,7 +1,7 @@`
`1`	`1`	`package frc.robot.constants;`
`2`	`2`
`3`	`3`	`public class PathHandlerConstants {`
`4`		`- public static final String[][] kpathNames = {`
	`4`	`+ public static final String[][] kShallowPathNames = {`
`5`	`5`	`{`
`6`	`6`	`"fetchToA",`
`7`	`7`	`"fetchToB",`