From 3201de14b4831e0db9f984e8ac845ffd7562ac83 Mon Sep 17 00:00:00 2001
From: Carter Briscoe <tha.c.bug@gmail.com>
Date: Tue, 25 Feb 2025 19:13:59 -0500
Subject: [PATCH 01/90] created poses D and C for the reef

---
 .../controls/drive/DriveControls.java         | 26 ++++++++++++++-----
 1 file changed, 20 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 9c1c8c08..7d3f2373 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -23,7 +23,9 @@ private enum TargetPoseOption {
     FEEDER_2(2),
     PROCESSOR(3),
     REEF_A(4),
-    REEF_G(5);
+    REEF_G(5),
+    REEF_C(6),
+    REEF_D(7);
 
     private int index;
 
@@ -78,11 +80,13 @@ public static void setupController(Drive drive, CommandXboxController controller
         poseReefA = new Pose2d(15, 4.175, Rotation2d.fromDegrees(180)),
         poseReefAClose = new Pose2d(14.5, 4.175, Rotation2d.fromDegrees(180)),
         poseReefG = new Pose2d(11, 4.175, Rotation2d.fromDegrees(0)),
-        poseReefGClose = new Pose2d(11.5, 4.175, Rotation2d.fromDegrees(0));
-    // PathConstraints constraints = new PathConstraints(4.2672, 9.4664784, 2 * Math.PI, 4 *
-    // Math.PI);
+        poseReefGClose = new Pose2d(11.5, 4.175, Rotation2d.fromDegrees(0)),
+        poseReefC = new Pose2d(14.17, 5.25, Rotation2d.fromDegrees(-124)),
+        poseReefD = new Pose2d(13.63, 5.56, Rotation2d.fromDegrees(-124));
+    PathConstraints constraints = new PathConstraints(4.2672, 9.4664784, 2 * Math.PI, 4 *
+    Math.PI);
     // PathConstraints constraints = new PathConstraints(2, 4.5, 2 * Math.PI, 4 * Math.PI);
-    PathConstraints constraints = new PathConstraints(0.5, 4.5, Math.PI / 4, 4 * Math.PI);
+    // PathConstraints constraints = new PathConstraints(0.5, 4.5, Math.PI / 4, 4 * Math.PI);
 
     // Temporary UI to allow user to modify destination on-the-fly
     SendableChooser<TargetPoseOption> chooser = new SendableChooser<>();
@@ -92,6 +96,8 @@ public static void setupController(Drive drive, CommandXboxController controller
     chooser.addOption("Processor", TargetPoseOption.PROCESSOR);
     chooser.addOption("Reef A", TargetPoseOption.REEF_A);
     chooser.addOption("Reef G", TargetPoseOption.REEF_G);
+    chooser.addOption("Reef C", TargetPoseOption.REEF_C);
+    chooser.addOption("Reef D", TargetPoseOption.REEF_D);
     SmartDashboard.putData("Pose choices", chooser);
 
     // Define behavior for chosing destination of on-the-fly pose
@@ -133,7 +139,15 @@ public static void setupController(Drive drive, CommandXboxController controller
                 Map.entry(
                     TargetPoseOption.REEF_G.getIndex(),
                     AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
-                        .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)))),
+                        .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
+                Map.entry(
+                    TargetPoseOption.REEF_C.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefC, constraints, 0.5)
+                        .andThen(AutoBuilder.pathfindToPose(poseReefC, constraints, 0.0))),
+                Map.entry(
+                    TargetPoseOption.REEF_D.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefD, constraints, 0.5)
+                        .andThen(AutoBuilder.pathfindToPose(poseReefD, constraints, 0.0)))),
             () -> {
               return myCoolPoseKeyIdx;
             });

From 0c0861b9f659de874a367a41acbfdc4b982b2207 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Tue, 25 Feb 2025 21:06:35 -0500
Subject: [PATCH 02/90] Pull from main and created an one piece auto that works
 well. I started improving the speed and this needs to bew tested.

---
 .pathplanner/settings.json                    |  6 +-
 simulation.properties                         |  2 +-
 .../deploy/pathplanner/autos/Go Straight.auto | 50 -----------
 .../deploy/pathplanner/autos/Score G L4.auto  | 52 ++++++++---
 .../paths/{Straight.path => Back G.path}      | 26 +++---
 .../deploy/pathplanner/paths/Get to G.path    | 18 ++--
 .../deploy/pathplanner/paths/Go Forward.path  |  6 +-
 .../pathplanner/paths/Start to Near G.path    | 10 +--
 src/main/deploy/pathplanner/settings.json     | 10 +--
 src/main/java/frc/robot/RobotContainer.java   |  3 +-
 .../game/reefscape2025/RobotConfig.java       |  3 +
 .../game/reefscape2025/RobotConfigNemo.java   | 17 +++-
 .../reefscape2025/RobotConfigPhoenix.java     | 13 ++-
 .../game/reefscape2025/RobotConfigStub.java   |  2 +-
 .../vision/VisionSubsystem.java               |  2 +-
 ...enix6-25.2.1.json => Phoenix6-25.3.1.json} | 88 +++++++++++++------
 ...b-2025.0.0.json => ReduxLib-2025.0.1.json} | 12 +--
 vendordeps/URCL.json                          | 10 +--
 vendordeps/maple-sim.json                     |  4 +-
 19 files changed, 184 insertions(+), 150 deletions(-)
 delete mode 100644 src/main/deploy/pathplanner/autos/Go Straight.auto
 rename src/main/deploy/pathplanner/paths/{Straight.path => Back G.path} (72%)
 rename vendordeps/{Phoenix6-25.2.1.json => Phoenix6-25.3.1.json} (86%)
 rename vendordeps/{ReduxLib-2025.0.0.json => ReduxLib-2025.0.1.json} (90%)

diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
index 3519769a..0276c8e2 100644
--- a/.pathplanner/settings.json
+++ b/.pathplanner/settings.json
@@ -1,10 +1,10 @@
 {
-  "robotWidth": 0.89,
-  "robotLength": 0.89,
+  "robotWidth": 0.7493,
+  "robotLength": 0.7493,
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 4.0,
+  "defaultMaxVel": 2.0,
   "defaultMaxAccel": 2.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
diff --git a/simulation.properties b/simulation.properties
index 2d763eed..b731d52c 100644
--- a/simulation.properties
+++ b/simulation.properties
@@ -1 +1 @@
-robot.name=STUB
\ No newline at end of file
+robot.name=NEMO
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Go Straight.auto b/src/main/deploy/pathplanner/autos/Go Straight.auto
deleted file mode 100644
index 5d7449c1..00000000
--- a/src/main/deploy/pathplanner/autos/Go Straight.auto	
+++ /dev/null
@@ -1,50 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Straight"
-          }
-        },
-        {
-          "type": "parallel",
-          "data": {
-            "commands": [
-              {
-                "type": "named",
-                "data": {
-                  "name": "Move Elevator"
-                }
-              },
-              {
-                "type": "named",
-                "data": {
-                  "name": "Move Arm High"
-                }
-              }
-            ]
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Go Straight 1 Meter"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Move Arm Low"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score G L4.auto b/src/main/deploy/pathplanner/autos/Score G L4.auto
index f36d58e9..5e0d8047 100644
--- a/src/main/deploy/pathplanner/autos/Score G L4.auto	
+++ b/src/main/deploy/pathplanner/autos/Score G L4.auto	
@@ -10,26 +10,46 @@
             "waitTime": 0
           }
         },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Start to Near G"
-          }
-        },
         {
           "type": "parallel",
           "data": {
             "commands": [
               {
-                "type": "named",
+                "type": "path",
                 "data": {
-                  "name": "Move Arm High"
+                  "pathName": "Start to Near G"
                 }
               },
               {
-                "type": "named",
+                "type": "sequential",
                 "data": {
-                  "name": "Move Elevator"
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.4 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
                 }
               }
             ]
@@ -38,13 +58,19 @@
         {
           "type": "named",
           "data": {
-            "name": "Go Straight 0.3 Meter"
+            "name": "Move Arm 0 degrees"
           }
         },
         {
-          "type": "named",
+          "type": "path",
+          "data": {
+            "pathName": "Get to G"
+          }
+        },
+        {
+          "type": "path",
           "data": {
-            "name": "Move Arm Low"
+            "pathName": "Back G"
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/paths/Straight.path b/src/main/deploy/pathplanner/paths/Back G.path
similarity index 72%
rename from src/main/deploy/pathplanner/paths/Straight.path
rename to src/main/deploy/pathplanner/paths/Back G.path
index 6a81612c..26ec3197 100644
--- a/src/main/deploy/pathplanner/paths/Straight.path
+++ b/src/main/deploy/pathplanner/paths/Back G.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.0,
-        "y": 1.0
+        "x": 5.75,
+        "y": 3.85
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.0,
-        "y": 1.0
+        "x": 6.0,
+        "y": 3.85
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.0,
-        "y": 1.0
+        "x": 6.2,
+        "y": 3.85
       },
       "prevControl": {
-        "x": 1.0,
-        "y": 1.0
+        "x": 5.95,
+        "y": 3.85
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,22 +33,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
-    "maxAngularVelocity": 2.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 1.5,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 180.0
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 180.0
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to G.path b/src/main/deploy/pathplanner/paths/Get to G.path
index 34f807e9..abc78a42 100644
--- a/src/main/deploy/pathplanner/paths/Get to G.path	
+++ b/src/main/deploy/pathplanner/paths/Get to G.path	
@@ -4,24 +4,24 @@
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.873
+        "y": 3.85
       },
       "prevControl": null,
       "nextControl": {
         "x": 5.95,
-        "y": 3.873
+        "y": 3.85
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.713721590909091,
-        "y": 3.873
+        "x": 5.75,
+        "y": 3.85
       },
       "prevControl": {
-        "x": 6.331960227272727,
-        "y": 3.8903835227272725
+        "x": 6.368238636363636,
+        "y": 3.8673835227272724
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,9 +33,9 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
-    "maxAngularVelocity": 2.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 1.5,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/Go Forward.path b/src/main/deploy/pathplanner/paths/Go Forward.path
index 45546292..c10aacb6 100644
--- a/src/main/deploy/pathplanner/paths/Go Forward.path	
+++ b/src/main/deploy/pathplanner/paths/Go Forward.path	
@@ -33,9 +33,9 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
-    "maxAngularVelocity": 2.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 1.5,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/Start to Near G.path b/src/main/deploy/pathplanner/paths/Start to Near G.path
index ade470d4..5f00ec0a 100644
--- a/src/main/deploy/pathplanner/paths/Start to Near G.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Near G.path	
@@ -17,11 +17,11 @@
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.873
+        "y": 3.85
       },
       "prevControl": {
         "x": 7.115994318181788,
-        "y": 3.8075596590909093
+        "y": 3.784559659090909
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,9 +33,9 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
-    "maxAngularVelocity": 2.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 1.5,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 1aa11407..42494837 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -1,12 +1,12 @@
 {
-  "robotWidth": 0.711,
-  "robotLength": 0.7112,
+  "robotWidth": 0.7493,
+  "robotLength": 0.7493,
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 2.5,
-  "defaultMaxAccel": 2.5,
-  "defaultMaxAngVel": 2.0,
+  "defaultMaxVel": 1.5,
+  "defaultMaxAccel": 1.5,
+  "defaultMaxAngVel": 1.5,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
   "robotMass": 74.088,
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index acb05886..34e322a2 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -14,13 +14,14 @@
 import java.io.IOException;
 import java.util.Properties;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+
 public class RobotContainer {
   public final RobotConfig robotConfig;
   private static final String robotNameKey = "Robot Name";
 
   public RobotContainer() {
     String robotName = "UNKNOWN";
-
     // Load robot name from configuration file
     // Check if the robot is running in simulation
     if (RobotBase.isSimulation()) {
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index fc9d21e7..cfcc7ac5 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -1,5 +1,7 @@
 package frc.robot.config.game.reefscape2025;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.controller.ArmFeedforward;
@@ -89,6 +91,7 @@ public RobotConfig(
     if (stubAuto) {
       autoChooser = new SendableChooser<>();
       autoChooser.setDefaultOption("No Auto Routines Specified", Commands.none());
+     
     }
 
     vision = new VisionSubsystem(AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape));
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
index 9fc5a997..fbbe99f6 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
@@ -1,5 +1,8 @@
 package frc.robot.config.game.reefscape2025;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
@@ -10,6 +13,8 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
+import frc.robot.commands.common.arm.ArmToPosition;
+import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.io.implementations.motor.MotorIOBase.MotorIOBaseSettings;
 import frc.robot.io.implementations.motor.MotorIOSparkMax;
 import frc.robot.io.implementations.motor.MotorIOSparkMax.SparkMaxSettings;
@@ -30,7 +35,7 @@
 /* Override Nemo specific constants here */
 public class RobotConfigNemo extends RobotConfig {
   public RobotConfigNemo() {
-    super(false, true, true, false, false, true, false);
+    super(false, false, false, false, false, false, false);
 
     // Nemo has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
@@ -126,7 +131,7 @@ public RobotConfigNemo() {
       motorSettings.reverseLimitNegate = true;
       motorSettings.motor.drumRadiusMeters =
           Units.inchesToMeters(
-              ((1.75 + 0.75) / 2)
+              ((1.10 + 0.75) / 2)
                   / 2); // 3/4" inner diameter to 1 3/4" outer. Average diameter calculated For now,
       // use the diameter so that
       // we don't reach the limits
@@ -196,5 +201,13 @@ public RobotConfigNemo() {
               // new MotorIOArmStub(motorSettings, armSettings), "Coral", armSettings);
               new MotorIOSparkMax(motorSettings, settings), "Climber", armSettings);
     }
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.5 meter", new ElevatorToPosition(elevator, () -> 0.5));
+        NamedCommands.registerCommand(
+        "Move Elevator to 1.4 meter", new ElevatorToPosition(elevator, () -> 1.4));
+    NamedCommands.registerCommand("Move Arm 75 degrees", new ArmToPosition(coralArm, () -> 70).withTimeout(1.5));
+    NamedCommands.registerCommand("Move Arm 0 degrees", new ArmToPosition(coralArm, () -> 0).withTimeout(1.5));
+    autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
+
   }
 }
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
index 99258acf..01f44ca5 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
@@ -1,11 +1,16 @@
 package frc.robot.config.game.reefscape2025;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import frc.robot.commands.common.arm.ArmToPosition;
+import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.subsystems.implementations.drive.DriveBase;
 import frc.robot.subsystems.implementations.drive.DriveSwerveYAGSL;
 import frc.robot.subsystems.interfaces.Drive;
@@ -14,7 +19,7 @@
 /* Override Phoenix specific constants here */
 public class RobotConfigPhoenix extends RobotConfig {
   public RobotConfigPhoenix() {
-    super(false, true, false, true, true, true);
+    super(false, false, false, true, true, true);
 
     // Phoenix has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
@@ -30,5 +35,11 @@ public RobotConfigPhoenix() {
             new Transform3d(
                 new Translation3d(Units.inchesToMeters(13.25), 0, Units.inchesToMeters(6.25)),
                 new Rotation3d(0, -Units.degreesToRadians(15), 0))));
+
+                NamedCommands.registerCommand(
+        "Move Elevator", new ElevatorToPosition(RobotConfig.elevator, () -> 1.8288));
+    NamedCommands.registerCommand("Move Arm High", new ArmToPosition(RobotConfig.coralArm, () -> 135.0));
+    NamedCommands.registerCommand("Move Arm Low", new ArmToPosition(RobotConfig.coralArm, () -> 90.0));
+    autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
   }
 }
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigStub.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigStub.java
index 9f67da17..0867ff05 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigStub.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigStub.java
@@ -27,7 +27,7 @@ public class RobotConfigStub extends RobotConfig {
   private final FlywheelMotorSubsystem algaeIntake;
 
   public RobotConfigStub() {
-    super(false, true, false);
+    super(false, false, false);
 
     drive = new DriveSwerveYAGSL("yagsl/stub");
     if (Robot.isSimulation()) {
diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index beee924f..98efacd7 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -178,7 +178,7 @@ public void updatePoseEstimation() {
         double distanceToBestTarget = camera.getDistanceToBestTarget();
 
         // Add vision measurement to the consumer.
-        if (visionMeasurementConsumer != null && distanceToBestTarget > 5) {
+        if (visionMeasurementConsumer != null && distanceToBestTarget < 5) {
           visionMeasurementConsumer.add(
               currentEstimatedRobotPose.get().estimatedPose.toPose2d(),
               currentEstimatedRobotPose.get().timestampSeconds,
diff --git a/vendordeps/Phoenix6-25.2.1.json b/vendordeps/Phoenix6-25.3.1.json
similarity index 86%
rename from vendordeps/Phoenix6-25.2.1.json
rename to vendordeps/Phoenix6-25.3.1.json
index 1397da1b..a216d975 100644
--- a/vendordeps/Phoenix6-25.2.1.json
+++ b/vendordeps/Phoenix6-25.3.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.2.1.json",
+    "fileName": "Phoenix6-25.3.1.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.2.1",
+    "version": "25.3.1",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.2.1"
+            "version": "25.3.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +152,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -194,7 +194,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.1",
+            "version": "25.3.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdi",
+            "version": "25.3.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -210,7 +224,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -226,7 +240,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -242,7 +256,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -258,7 +272,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -274,7 +288,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -290,7 +304,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -306,7 +320,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -322,7 +336,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -338,7 +352,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -354,7 +368,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_SimProTalonFXS",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -370,7 +384,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -386,7 +400,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -402,7 +416,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.1",
+            "version": "25.3.1",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -414,6 +428,22 @@
                 "osxuniversal"
             ],
             "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdi",
+            "version": "25.3.1",
+            "libName": "CTRE_SimProCANdi",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
         }
     ]
 }
\ No newline at end of file
diff --git a/vendordeps/ReduxLib-2025.0.0.json b/vendordeps/ReduxLib-2025.0.1.json
similarity index 90%
rename from vendordeps/ReduxLib-2025.0.0.json
rename to vendordeps/ReduxLib-2025.0.1.json
index f15ff901..6cc750e5 100644
--- a/vendordeps/ReduxLib-2025.0.0.json
+++ b/vendordeps/ReduxLib-2025.0.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "ReduxLib-2025.0.0.json",
+    "fileName": "ReduxLib-2025.0.1.json",
     "name": "ReduxLib",
-    "version": "2025.0.0",
+    "version": "2025.0.1",
     "frcYear": "2025",
     "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.reduxrobotics.frc",
             "artifactId": "ReduxLib-java",
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.reduxrobotics.frc",
             "artifactId": "ReduxLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -36,7 +36,7 @@
         {
             "groupId": "com.reduxrobotics.frc",
             "artifactId": "ReduxLib-cpp",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "libName": "ReduxLib",
             "headerClassifier": "headers",
             "sourcesClassifier": "sources",
@@ -54,7 +54,7 @@
         {
             "groupId": "com.reduxrobotics.frc",
             "artifactId": "ReduxLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "libName": "ReduxCore",
             "headerClassifier": "headers",
             "sharedLibrary": true,
diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json
index 55e7841a..2310eee9 100644
--- a/vendordeps/URCL.json
+++ b/vendordeps/URCL.json
@@ -1,7 +1,7 @@
 {
     "fileName": "URCL.json",
     "name": "URCL",
-    "version": "2025.0.0",
+    "version": "2025.0.1",
     "frcYear": "2025",
     "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "org.littletonrobotics.urcl",
             "artifactId": "URCL-java",
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "org.littletonrobotics.urcl",
             "artifactId": "URCL-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -34,7 +34,7 @@
         {
             "groupId": "org.littletonrobotics.urcl",
             "artifactId": "URCL-cpp",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "libName": "URCL",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -49,7 +49,7 @@
         {
             "groupId": "org.littletonrobotics.urcl",
             "artifactId": "URCL-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "libName": "URCLDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json
index 5369ef8b..579c2609 100644
--- a/vendordeps/maple-sim.json
+++ b/vendordeps/maple-sim.json
@@ -1,7 +1,7 @@
 {
     "fileName": "maple-sim.json",
     "name": "maplesim",
-    "version": "0.3.1",
+    "version": "0.3.8",
     "frcYear": "2025",
     "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b",
     "mavenUrls": [
@@ -13,7 +13,7 @@
         {
             "groupId": "org.ironmaple",
             "artifactId": "maplesim-java",
-            "version": "0.3.1"
+            "version": "0.3.8"
         },
         {
             "groupId": "org.dyn4j",

From abc31ca4faeb3eff559f664148761358d97ae6b9 Mon Sep 17 00:00:00 2001
From: Carter Briscoe <tha.c.bug@gmail.com>
Date: Wed, 26 Feb 2025 20:32:28 -0500
Subject: [PATCH 03/90] made pathplanner positions more accurate

---
 .../controls/drive/DriveControls.java         | 37 +++++++++++++------
 .../drive/DriveSwerveYAGSL.java               |  4 +-
 .../vision/VisionSubsystem.java               |  2 +-
 3 files changed, 29 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 7d3f2373..c8ed503b 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -25,7 +25,9 @@ private enum TargetPoseOption {
     REEF_A(4),
     REEF_G(5),
     REEF_C(6),
-    REEF_D(7);
+    REEF_D(7),
+    WOW_Test(8),
+    WOW_Test1(9);
 
     private int index;
 
@@ -82,10 +84,12 @@ public static void setupController(Drive drive, CommandXboxController controller
         poseReefG = new Pose2d(11, 4.175, Rotation2d.fromDegrees(0)),
         poseReefGClose = new Pose2d(11.5, 4.175, Rotation2d.fromDegrees(0)),
         poseReefC = new Pose2d(14.17, 5.25, Rotation2d.fromDegrees(-124)),
-        poseReefD = new Pose2d(13.63, 5.56, Rotation2d.fromDegrees(-124));
-    PathConstraints constraints = new PathConstraints(4.2672, 9.4664784, 2 * Math.PI, 4 *
-    Math.PI);
-    // PathConstraints constraints = new PathConstraints(2, 4.5, 2 * Math.PI, 4 * Math.PI);
+        poseReefD = new Pose2d(13.63, 5.56, Rotation2d.fromDegrees(-124)),
+        poseTest = new Pose2d(2.50, 5.57, Rotation2d.fromDegrees(-124)),
+        poseTest2 = new Pose2d(2.46, 2.73, Rotation2d.fromDegrees(-124));
+    //  PathConstraints constraints = new PathConstraints(4.9672, 9.3664784, 2 * Math.PI, 4 *
+    //  Math.PI);
+    PathConstraints constraints = new PathConstraints(2, 1.5, 2 * Math.PI, 4 * Math.PI);
     // PathConstraints constraints = new PathConstraints(0.5, 4.5, Math.PI / 4, 4 * Math.PI);
 
     // Temporary UI to allow user to modify destination on-the-fly
@@ -98,6 +102,8 @@ public static void setupController(Drive drive, CommandXboxController controller
     chooser.addOption("Reef G", TargetPoseOption.REEF_G);
     chooser.addOption("Reef C", TargetPoseOption.REEF_C);
     chooser.addOption("Reef D", TargetPoseOption.REEF_D);
+    chooser.addOption("Test", TargetPoseOption.WOW_Test);
+    chooser.addOption("Test1", TargetPoseOption.WOW_Test1);
     SmartDashboard.putData("Pose choices", chooser);
 
     // Define behavior for chosing destination of on-the-fly pose
@@ -134,20 +140,29 @@ public static void setupController(Drive drive, CommandXboxController controller
                     AutoBuilder.pathfindToPose(poseProcessor, constraints, 0.0)),
                 Map.entry(
                     TargetPoseOption.REEF_A.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefA, constraints, 0.5)
+                    AutoBuilder.pathfindToPose(poseReefA, constraints, 0.0)
                         .andThen(AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0))),
                 Map.entry(
                     TargetPoseOption.REEF_G.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
-                        .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
+                    // AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
+                    //     .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
+                    AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)),
                 Map.entry(
                     TargetPoseOption.REEF_C.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefC, constraints, 0.5)
+                    AutoBuilder.pathfindToPose(poseReefC, constraints, 0.0)
                         .andThen(AutoBuilder.pathfindToPose(poseReefC, constraints, 0.0))),
                 Map.entry(
                     TargetPoseOption.REEF_D.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefD, constraints, 0.5)
-                        .andThen(AutoBuilder.pathfindToPose(poseReefD, constraints, 0.0)))),
+                    AutoBuilder.pathfindToPose(poseReefD, constraints, 0.0)
+                        .andThen(AutoBuilder.pathfindToPose(poseReefD, constraints, 0.0))),
+                Map.entry(
+                    TargetPoseOption.WOW_Test.getIndex(),
+                    AutoBuilder.pathfindToPose(poseTest, constraints, 0.0)
+                        .andThen(AutoBuilder.pathfindToPose(poseTest, constraints, 0.0))),
+                Map.entry(
+                    TargetPoseOption.WOW_Test1.getIndex(),
+                    AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0)
+                        .andThen(AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0)))),
             () -> {
               return myCoolPoseKeyIdx;
             });
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index 459c223f..a9db8bca 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -37,9 +37,9 @@ public static class Constants {
   private SwerveDrive swerveDrive;
   @AutoLogOutput private boolean fieldOrientedDrive = false;
   private PIDConstants translationPIDConstants =
-      new PIDConstants(5.0, 0.0, 0.0); // Translation PID constants
+      new PIDConstants(7.0, 0.0, 0.3); // Translation PID constants
   private PIDConstants rotationPIDConstants =
-      new PIDConstants(5.0, 0.0, 0.0); // Rotation PID constants
+      new PIDConstants(15.0, 0.0, 3.0); // Rotation PID constants
 
   // @AutoLogOutput
   DriveIO io = new DriveIO();
diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index beee924f..98efacd7 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -178,7 +178,7 @@ public void updatePoseEstimation() {
         double distanceToBestTarget = camera.getDistanceToBestTarget();
 
         // Add vision measurement to the consumer.
-        if (visionMeasurementConsumer != null && distanceToBestTarget > 5) {
+        if (visionMeasurementConsumer != null && distanceToBestTarget < 5) {
           visionMeasurementConsumer.add(
               currentEstimatedRobotPose.get().estimatedPose.toPose2d(),
               currentEstimatedRobotPose.get().timestampSeconds,

From 2c7be600e67d8b28885b15c2097032e815e155db Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Wed, 26 Feb 2025 21:05:47 -0500
Subject: [PATCH 04/90] Added 2 piece auto. More testing is needed.

---
 .../{Score G L4.auto => Score 1 G L4.auto}    |   8 +-
 .../pathplanner/autos/Score G L4 and L4 .auto | 170 ++++++++++++++++++
 src/main/deploy/pathplanner/paths/Back G.path |  12 +-
 .../deploy/pathplanner/paths/G to Source.path |  54 ++++++
 .../deploy/pathplanner/paths/Get to G.path    |  12 +-
 .../deploy/pathplanner/paths/New Path.path    |  28 +--
 .../pathplanner/paths/Source to temp.path     |  54 ++++++
 src/main/java/frc/robot/RobotContainer.java   |   2 -
 .../game/reefscape2025/RobotConfig.java       |   3 -
 .../game/reefscape2025/RobotConfigNemo.java   |  16 +-
 .../reefscape2025/RobotConfigPhoenix.java     |   9 +-
 11 files changed, 324 insertions(+), 44 deletions(-)
 rename src/main/deploy/pathplanner/autos/{Score G L4.auto => Score 1 G L4.auto} (100%)
 create mode 100644 src/main/deploy/pathplanner/autos/Score G L4 and L4 .auto
 create mode 100644 src/main/deploy/pathplanner/paths/G to Source.path
 create mode 100644 src/main/deploy/pathplanner/paths/Source to temp.path

diff --git a/src/main/deploy/pathplanner/autos/Score G L4.auto b/src/main/deploy/pathplanner/autos/Score 1 G L4.auto
similarity index 100%
rename from src/main/deploy/pathplanner/autos/Score G L4.auto
rename to src/main/deploy/pathplanner/autos/Score 1 G L4.auto
index 5e0d8047..4d392d66 100644
--- a/src/main/deploy/pathplanner/autos/Score G L4.auto	
+++ b/src/main/deploy/pathplanner/autos/Score 1 G L4.auto	
@@ -56,15 +56,15 @@
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "Move Arm 0 degrees"
+            "pathName": "Get to G"
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Get to G"
+            "name": "Move Arm 0 degrees"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/Score G L4 and L4 .auto b/src/main/deploy/pathplanner/autos/Score G L4 and L4 .auto
new file mode 100644
index 00000000..43b442c6
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score G L4 and L4 .auto	
@@ -0,0 +1,170 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near G"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.4 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to G"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back G"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "G to Source"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Elevator to 0.6 meter"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Arm for Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 4.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Elevator to 0.2 meter"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Source to temp"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.4 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back G.path b/src/main/deploy/pathplanner/paths/Back G.path
index 26ec3197..6822cc0c 100644
--- a/src/main/deploy/pathplanner/paths/Back G.path	
+++ b/src/main/deploy/pathplanner/paths/Back G.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.75,
-        "y": 3.85
+        "x": 5.9,
+        "y": 3.8
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.0,
-        "y": 3.85
+        "x": 6.15,
+        "y": 3.8
       },
       "isLocked": false,
       "linkedName": null
@@ -17,11 +17,11 @@
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.85
+        "y": 3.8
       },
       "prevControl": {
         "x": 5.95,
-        "y": 3.85
+        "y": 3.8
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/G to Source.path b/src/main/deploy/pathplanner/paths/G to Source.path
new file mode 100644
index 00000000..ef7bf31d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/G to Source.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.122556818181818,
+        "y": 3.950213068181818
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.149630681818182,
+        "y": 5.266463068181817
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2165340909090911,
+        "y": 7.021463068181817
+      },
+      "prevControl": {
+        "x": 3.8789488636363636,
+        "y": 6.8818607954545445
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 1.5,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -56.976000000000006
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -178.85423716182487
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to G.path b/src/main/deploy/pathplanner/paths/Get to G.path
index abc78a42..be61795e 100644
--- a/src/main/deploy/pathplanner/paths/Get to G.path	
+++ b/src/main/deploy/pathplanner/paths/Get to G.path	
@@ -4,24 +4,24 @@
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.85
+        "y": 3.8
       },
       "prevControl": null,
       "nextControl": {
         "x": 5.95,
-        "y": 3.85
+        "y": 3.8
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.75,
-        "y": 3.85
+        "x": 5.9,
+        "y": 3.8
       },
       "prevControl": {
-        "x": 6.368238636363636,
-        "y": 3.8673835227272724
+        "x": 6.149810079096521,
+        "y": 3.7902570855597366
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
index 9202a4ab..2e5404c9 100644
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.122556818181818,
-        "y": 3.950213068181818
+        "x": 3.61,
+        "y": 5.177
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.661022727272727,
-        "y": 5.50578125
+        "x": 4.609999999999999,
+        "y": 5.177
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.8946022727272724,
-        "y": 6.5
+        "x": 4.0,
+        "y": 6.0
       },
       "prevControl": {
-        "x": 4.557017045454547,
-        "y": 6.639602272727273
+        "x": 3.0,
+        "y": 6.0
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,22 +33,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.2672,
-    "maxAcceleration": 9.466478,
-    "maxAngularVelocity": 30.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 1.5,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 141.61670272897086
+    "rotation": 0.0
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -178.85423716182487
+    "rotation": 0.0
   },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Source to temp.path b/src/main/deploy/pathplanner/paths/Source to temp.path
new file mode 100644
index 00000000..60879b87
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Source to temp.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.227,
+        "y": 7.091
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.3233806818181817,
+        "y": 6.094105113636363
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.61,
+        "y": 5.17671875
+      },
+      "prevControl": {
+        "x": 2.61,
+        "y": 5.17671875
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 1.5,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -57.265
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -56.976000000000006
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 34e322a2..a2e4c1bc 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -14,8 +14,6 @@
 import java.io.IOException;
 import java.util.Properties;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-
 public class RobotContainer {
   public final RobotConfig robotConfig;
   private static final String robotNameKey = "Robot Name";
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index cfcc7ac5..fc9d21e7 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -1,7 +1,5 @@
 package frc.robot.config.game.reefscape2025;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.controller.ArmFeedforward;
@@ -91,7 +89,6 @@ public RobotConfig(
     if (stubAuto) {
       autoChooser = new SendableChooser<>();
       autoChooser.setDefaultOption("No Auto Routines Specified", Commands.none());
-     
     }
 
     vision = new VisionSubsystem(AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape));
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
index fbbe99f6..29a25aec 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
@@ -2,7 +2,6 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
@@ -203,11 +202,18 @@ public RobotConfigNemo() {
     }
     NamedCommands.registerCommand(
         "Move Elevator to 0.5 meter", new ElevatorToPosition(elevator, () -> 0.5));
-        NamedCommands.registerCommand(
+    NamedCommands.registerCommand(
         "Move Elevator to 1.4 meter", new ElevatorToPosition(elevator, () -> 1.4));
-    NamedCommands.registerCommand("Move Arm 75 degrees", new ArmToPosition(coralArm, () -> 70).withTimeout(1.5));
-    NamedCommands.registerCommand("Move Arm 0 degrees", new ArmToPosition(coralArm, () -> 0).withTimeout(1.5));
+    NamedCommands.registerCommand(
+        "Move Arm 75 degrees", new ArmToPosition(coralArm, () -> 70).withTimeout(1.5));
+    NamedCommands.registerCommand(
+        "Move Arm 0 degrees", new ArmToPosition(coralArm, () -> 0).withTimeout(1.5));
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.6 meter", new ElevatorToPosition(elevator, () -> 0.6));
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.2 meter", new ElevatorToPosition(elevator, () -> 0.2));
+    NamedCommands.registerCommand(
+        "Move Arm for Intake", new ArmToPosition(coralArm, () -> -90).withTimeout(0));
     autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
-
   }
 }
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
index 01f44ca5..154c4ecf 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
@@ -2,7 +2,6 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform2d;
@@ -36,10 +35,12 @@ public RobotConfigPhoenix() {
                 new Translation3d(Units.inchesToMeters(13.25), 0, Units.inchesToMeters(6.25)),
                 new Rotation3d(0, -Units.degreesToRadians(15), 0))));
 
-                NamedCommands.registerCommand(
+    NamedCommands.registerCommand(
         "Move Elevator", new ElevatorToPosition(RobotConfig.elevator, () -> 1.8288));
-    NamedCommands.registerCommand("Move Arm High", new ArmToPosition(RobotConfig.coralArm, () -> 135.0));
-    NamedCommands.registerCommand("Move Arm Low", new ArmToPosition(RobotConfig.coralArm, () -> 90.0));
+    NamedCommands.registerCommand(
+        "Move Arm High", new ArmToPosition(RobotConfig.coralArm, () -> 135.0));
+    NamedCommands.registerCommand(
+        "Move Arm Low", new ArmToPosition(RobotConfig.coralArm, () -> 90.0));
     autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
   }
 }

From 5289ebf16b913c09d82649057d8026dc95cfdd8c Mon Sep 17 00:00:00 2001
From: Carter Briscoe <tha.c.bug@gmail.com>
Date: Thu, 27 Feb 2025 18:04:52 -0500
Subject: [PATCH 05/90] vision change

---
 src/main/deploy/pathplanner/navgrid.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index af0de35c..23e0db94 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file

From 02bf47d445f23f779c0ab86121de40adb861a6f5 Mon Sep 17 00:00:00 2001
From: Carter Briscoe <tha.c.bug@gmail.com>
Date: Thu, 27 Feb 2025 20:55:10 -0500
Subject: [PATCH 06/90] Updates to drive assist values

---
 .../controls/drive/DriveControls.java          | 18 ++++++++++--------
 .../drive/DriveSwerveYAGSL.java                |  8 ++++++--
 .../vision/VisionSubsystem.java                |  8 ++++----
 3 files changed, 20 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index c8ed503b..be296d81 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -76,11 +76,11 @@ public static void setupController(Drive drive, CommandXboxController controller
 
     // Define destinations for our "dynamic go-to-pose" functionality
     Pose2d poseOrigin = new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-        poseFeeder1 = new Pose2d(16.02, 6.9, Rotation2d.fromDegrees(50)),
+        poseFeeder1 = new Pose2d(16.44, 7.25, Rotation2d.fromDegrees(230)),
         poseFeeder2 = new Pose2d(16.02, 1, Rotation2d.fromDegrees(-50)),
         poseProcessor = new Pose2d(11.5, 7.3, Rotation2d.fromDegrees(90)),
         poseReefA = new Pose2d(15, 4.175, Rotation2d.fromDegrees(180)),
-        poseReefAClose = new Pose2d(14.5, 4.175, Rotation2d.fromDegrees(180)),
+        poseReefAClose = new Pose2d(14.425, 4.175, Rotation2d.fromDegrees(180)),
         poseReefG = new Pose2d(11, 4.175, Rotation2d.fromDegrees(0)),
         poseReefGClose = new Pose2d(11.5, 4.175, Rotation2d.fromDegrees(0)),
         poseReefC = new Pose2d(14.17, 5.25, Rotation2d.fromDegrees(-124)),
@@ -89,7 +89,8 @@ public static void setupController(Drive drive, CommandXboxController controller
         poseTest2 = new Pose2d(2.46, 2.73, Rotation2d.fromDegrees(-124));
     //  PathConstraints constraints = new PathConstraints(4.9672, 9.3664784, 2 * Math.PI, 4 *
     //  Math.PI);
-    PathConstraints constraints = new PathConstraints(2, 1.5, 2 * Math.PI, 4 * Math.PI);
+    // PathConstraints constraints = new PathConstraints(2, 1.5, 2 * Math.PI, 4 * Math.PI);
+    PathConstraints constraints = new PathConstraints(2, 1.5, Math.PI / 2, Math.PI / 4);
     // PathConstraints constraints = new PathConstraints(0.5, 4.5, Math.PI / 4, 4 * Math.PI);
 
     // Temporary UI to allow user to modify destination on-the-fly
@@ -140,13 +141,14 @@ public static void setupController(Drive drive, CommandXboxController controller
                     AutoBuilder.pathfindToPose(poseProcessor, constraints, 0.0)),
                 Map.entry(
                     TargetPoseOption.REEF_A.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefA, constraints, 0.0)
-                        .andThen(AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0))),
+                    AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0)),
+                    // AutoBuilder.pathfindToPose(poseReefA, constraints, 0.0)
+                    //     .andThen(AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0))),
                 Map.entry(
                     TargetPoseOption.REEF_G.getIndex(),
-                    // AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
-                    //     .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
-                    AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)),
+                     AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
+                         .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
+                    //AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)),
                 Map.entry(
                     TargetPoseOption.REEF_C.getIndex(),
                     AutoBuilder.pathfindToPose(poseReefC, constraints, 0.0)
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index a9db8bca..2b4e0680 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -36,10 +36,14 @@ public static class Constants {
   private final File swerveJsonDirectory;
   private SwerveDrive swerveDrive;
   @AutoLogOutput private boolean fieldOrientedDrive = false;
+  // private PIDConstants translationPIDConstants =
+  //     new PIDConstants(5.0, 0.0, 0.0); // Translation PID constants
+  private PIDConstants rotationPIDConstants =
+      new PIDConstants(8.0, 0.0, 1.0); // Rotation PID constants
   private PIDConstants translationPIDConstants =
       new PIDConstants(7.0, 0.0, 0.3); // Translation PID constants
-  private PIDConstants rotationPIDConstants =
-      new PIDConstants(15.0, 0.0, 3.0); // Rotation PID constants
+  // private PIDConstants rotationPIDConstants =
+  //     new PIDConstants(15.0, 0.0, 3.0); // Rotation PID constants
 
   // @AutoLogOutput
   DriveIO io = new DriveIO();
diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index 98efacd7..a2b99002 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -83,7 +83,7 @@ private double getDistanceToBestTarget() {
       return PhotonUtils.calculateDistanceToTargetMeters(
               robotToCamera.getZ(),
               fieldLayout.getTagPose(bestTarget.getFiducialId()).get().getZ(),
-              robotToCamera.getRotation().getY(),
+              -robotToCamera.getRotation().getY(),
               Units.degreesToRadians(bestTarget.getPitch()))
           + Constants.visionDistanceOffsetInMeters;
     }
@@ -175,15 +175,15 @@ public void updatePoseEstimation() {
 
       Optional<EstimatedRobotPose> currentEstimatedRobotPose = camera.getEstimatedRobotPose();
       if (currentEstimatedRobotPose.isPresent()) {
-        double distanceToBestTarget = camera.getDistanceToBestTarget();
+        debugTargetDistance = camera.getDistanceToBestTarget();
 
         // Add vision measurement to the consumer.
-        if (visionMeasurementConsumer != null && distanceToBestTarget < 5) {
+        if (visionMeasurementConsumer != null && debugTargetDistance < 0.8) {
           visionMeasurementConsumer.add(
               currentEstimatedRobotPose.get().estimatedPose.toPose2d(),
               currentEstimatedRobotPose.get().timestampSeconds,
               VecBuilder.fill(
-                  distanceToBestTarget / 2, distanceToBestTarget / 2, distanceToBestTarget / 2));
+                debugTargetDistance / 2, debugTargetDistance / 2, debugTargetDistance / 2));
         }
         /* NOTE standard deviation format:
          * (x position in meters, y position in meters, and heading in radians)

From b991e0258925896244968509560b3601363ae17c Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Fri, 28 Feb 2025 18:58:10 -0500
Subject: [PATCH 07/90] wpilib update and spotless

---
 build.gradle                                           |  2 +-
 .../robot/subsystems/controls/drive/DriveControls.java | 10 +++++-----
 .../implementations/vision/VisionSubsystem.java        |  2 +-
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/build.gradle b/build.gradle
index 7424e2bc..e141a6ca 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.2.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.1"
     id "com.diffplug.spotless" version "6.23.3"
     id "com.peterabeles.gversion" version "1.10.2"
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index be296d81..739cc797 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -142,13 +142,13 @@ public static void setupController(Drive drive, CommandXboxController controller
                 Map.entry(
                     TargetPoseOption.REEF_A.getIndex(),
                     AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0)),
-                    // AutoBuilder.pathfindToPose(poseReefA, constraints, 0.0)
-                    //     .andThen(AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0))),
+                // AutoBuilder.pathfindToPose(poseReefA, constraints, 0.0)
+                //     .andThen(AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0))),
                 Map.entry(
                     TargetPoseOption.REEF_G.getIndex(),
-                     AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
-                         .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
-                    //AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)),
+                    AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
+                        .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
+                // AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)),
                 Map.entry(
                     TargetPoseOption.REEF_C.getIndex(),
                     AutoBuilder.pathfindToPose(poseReefC, constraints, 0.0)
diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index a2b99002..75001de8 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -183,7 +183,7 @@ public void updatePoseEstimation() {
               currentEstimatedRobotPose.get().estimatedPose.toPose2d(),
               currentEstimatedRobotPose.get().timestampSeconds,
               VecBuilder.fill(
-                debugTargetDistance / 2, debugTargetDistance / 2, debugTargetDistance / 2));
+                  debugTargetDistance / 2, debugTargetDistance / 2, debugTargetDistance / 2));
         }
         /* NOTE standard deviation format:
          * (x position in meters, y position in meters, and heading in radians)

From b852587541f722e012dfa324ba119c85f8a1c815 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Fri, 28 Feb 2025 19:00:47 -0500
Subject: [PATCH 08/90] fix reference to field layout

---
 .../java/frc/robot/config/game/reefscape2025/RobotConfig.java  | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index fc9d21e7..a5f3d850 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -91,7 +91,8 @@ public RobotConfig(
       autoChooser.setDefaultOption("No Auto Routines Specified", Commands.none());
     }
 
-    vision = new VisionSubsystem(AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape));
+    vision =
+        new VisionSubsystem(AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark));
 
     if (stubVision) {
       if (Robot.isSimulation()) {

From a63e415a1b77ddef4a967aece2067cfd9ebdbedc Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Fri, 28 Feb 2025 20:31:50 -0500
Subject: [PATCH 09/90] fixed positions, modified button toggle for two
 positions

---
 .../controls/drive/DriveControls.java         | 23 ++++++++++++-------
 .../drive/DriveSwerveYAGSL.java               |  8 +++----
 2 files changed, 19 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 739cc797..dcc8b43b 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -26,8 +26,9 @@ private enum TargetPoseOption {
     REEF_G(5),
     REEF_C(6),
     REEF_D(7),
-    WOW_Test(8),
-    WOW_Test1(9);
+    REEF_L(8),
+    WOW_Test(9),
+    WOW_Test1(10);
 
     private int index;
 
@@ -77,7 +78,7 @@ public static void setupController(Drive drive, CommandXboxController controller
     // Define destinations for our "dynamic go-to-pose" functionality
     Pose2d poseOrigin = new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
         poseFeeder1 = new Pose2d(16.44, 7.25, Rotation2d.fromDegrees(230)),
-        poseFeeder2 = new Pose2d(16.02, 1, Rotation2d.fromDegrees(-50)),
+        poseFeeder2 = new Pose2d(16.02, 1, Rotation2d.fromDegrees(-230)),
         poseProcessor = new Pose2d(11.5, 7.3, Rotation2d.fromDegrees(90)),
         poseReefA = new Pose2d(15, 4.175, Rotation2d.fromDegrees(180)),
         poseReefAClose = new Pose2d(14.425, 4.175, Rotation2d.fromDegrees(180)),
@@ -85,6 +86,7 @@ public static void setupController(Drive drive, CommandXboxController controller
         poseReefGClose = new Pose2d(11.5, 4.175, Rotation2d.fromDegrees(0)),
         poseReefC = new Pose2d(14.17, 5.25, Rotation2d.fromDegrees(-124)),
         poseReefD = new Pose2d(13.63, 5.56, Rotation2d.fromDegrees(-124)),
+        poseReefL = new Pose2d(14.04, 2.82, Rotation2d.fromDegrees(124)),
         poseTest = new Pose2d(2.50, 5.57, Rotation2d.fromDegrees(-124)),
         poseTest2 = new Pose2d(2.46, 2.73, Rotation2d.fromDegrees(-124));
     //  PathConstraints constraints = new PathConstraints(4.9672, 9.3664784, 2 * Math.PI, 4 *
@@ -103,6 +105,7 @@ public static void setupController(Drive drive, CommandXboxController controller
     chooser.addOption("Reef G", TargetPoseOption.REEF_G);
     chooser.addOption("Reef C", TargetPoseOption.REEF_C);
     chooser.addOption("Reef D", TargetPoseOption.REEF_D);
+    chooser.addOption("Reef L", TargetPoseOption.REEF_L);
     chooser.addOption("Test", TargetPoseOption.WOW_Test);
     chooser.addOption("Test1", TargetPoseOption.WOW_Test1);
     SmartDashboard.putData("Pose choices", chooser);
@@ -119,7 +122,10 @@ public static void setupController(Drive drive, CommandXboxController controller
         .onTrue(
             new InstantCommand(
                 () -> {
-                  if (++myCoolPoseKeyIdx == TargetPoseOption.values().length) myCoolPoseKeyIdx = 1;
+                  if (myCoolPoseKeyIdx == 8) myCoolPoseKeyIdx = 2;
+                  else myCoolPoseKeyIdx = 8;
+                  //   if (++myCoolPoseKeyIdx == TargetPoseOption.values().length) myCoolPoseKeyIdx
+                  // = 1;
                   SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
                 }));
 
@@ -151,12 +157,13 @@ public static void setupController(Drive drive, CommandXboxController controller
                 // AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)),
                 Map.entry(
                     TargetPoseOption.REEF_C.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefC, constraints, 0.0)
-                        .andThen(AutoBuilder.pathfindToPose(poseReefC, constraints, 0.0))),
+                    AutoBuilder.pathfindToPose(poseReefC, constraints, 0.0)),
                 Map.entry(
                     TargetPoseOption.REEF_D.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefD, constraints, 0.0)
-                        .andThen(AutoBuilder.pathfindToPose(poseReefD, constraints, 0.0))),
+                    AutoBuilder.pathfindToPose(poseReefD, constraints, 0.0)),
+                Map.entry(
+                    TargetPoseOption.REEF_L.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefL, constraints, 0.0)),
                 Map.entry(
                     TargetPoseOption.WOW_Test.getIndex(),
                     AutoBuilder.pathfindToPose(poseTest, constraints, 0.0)
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index 2b4e0680..4fc75d07 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -38,12 +38,12 @@ public static class Constants {
   @AutoLogOutput private boolean fieldOrientedDrive = false;
   // private PIDConstants translationPIDConstants =
   //     new PIDConstants(5.0, 0.0, 0.0); // Translation PID constants
-  private PIDConstants rotationPIDConstants =
-      new PIDConstants(8.0, 0.0, 1.0); // Rotation PID constants
+  // private PIDConstants rotationPIDConstants =
+  //     new PIDConstants(8.0, 0.0, 1.0); // Rotation PID constants
   private PIDConstants translationPIDConstants =
       new PIDConstants(7.0, 0.0, 0.3); // Translation PID constants
-  // private PIDConstants rotationPIDConstants =
-  //     new PIDConstants(15.0, 0.0, 3.0); // Rotation PID constants
+  private PIDConstants rotationPIDConstants =
+      new PIDConstants(5.0, 0.0, 0.0); // Rotation PID constants
 
   // @AutoLogOutput
   DriveIO io = new DriveIO();

From bc5ce0e2f7bae484dff24e7be7ef01a14b213eb9 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Sat, 1 Mar 2025 12:26:01 -0500
Subject: [PATCH 10/90] Changed values with acccurate AprilTag placement Need
 to fix second piece auto

---
 .../pathplanner/autos/Score 1 G L4.auto       |  2 +-
 ...d L4 .auto => Score G L4 + Y + L L4 .auto} | 16 ++++++----
 src/main/deploy/pathplanner/paths/Back G.path | 24 +++++++--------
 .../{Source to temp.path => Back L.path}      | 28 ++++++++---------
 .../{G to Source.path => G to Source Y.path}  | 30 +++++++++----------
 .../deploy/pathplanner/paths/Get to G.path    | 24 +++++++--------
 .../deploy/pathplanner/paths/Go Forward.path  |  6 ++--
 .../{New Path.path => Source Y to L.path}     | 30 +++++++++----------
 .../pathplanner/paths/Start to Near G.path    | 20 ++++++-------
 src/main/deploy/pathplanner/settings.json     | 10 +++----
 .../game/reefscape2025/RobotConfigNemo.java   | 11 ++++---
 .../combination/DriverAssistControls.java     |  8 ++---
 12 files changed, 107 insertions(+), 102 deletions(-)
 rename src/main/deploy/pathplanner/autos/{Score G L4 and L4 .auto => Score G L4 + Y + L L4 .auto} (91%)
 rename src/main/deploy/pathplanner/paths/{Source to temp.path => Back L.path} (70%)
 rename src/main/deploy/pathplanner/paths/{G to Source.path => G to Source Y.path} (59%)
 rename src/main/deploy/pathplanner/paths/{New Path.path => Source Y to L.path} (62%)

diff --git a/src/main/deploy/pathplanner/autos/Score 1 G L4.auto b/src/main/deploy/pathplanner/autos/Score 1 G L4.auto
index 4d392d66..276e2b6b 100644
--- a/src/main/deploy/pathplanner/autos/Score 1 G L4.auto	
+++ b/src/main/deploy/pathplanner/autos/Score 1 G L4.auto	
@@ -43,7 +43,7 @@
                           {
                             "type": "named",
                             "data": {
-                              "name": "Move Elevator to 1.4 meter"
+                              "name": "Move Elevator to 1.553 meter"
                             }
                           }
                         ]
diff --git a/src/main/deploy/pathplanner/autos/Score G L4 and L4 .auto b/src/main/deploy/pathplanner/autos/Score G L4 + Y + L L4 .auto
similarity index 91%
rename from src/main/deploy/pathplanner/autos/Score G L4 and L4 .auto
rename to src/main/deploy/pathplanner/autos/Score G L4 + Y + L L4 .auto
index 43b442c6..169cbafa 100644
--- a/src/main/deploy/pathplanner/autos/Score G L4 and L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score G L4 + Y + L L4 .auto	
@@ -43,7 +43,7 @@
                           {
                             "type": "named",
                             "data": {
-                              "name": "Move Elevator to 1.4 meter"
+                              "name": "Move Elevator to 1.553 meter"
                             }
                           }
                         ]
@@ -80,13 +80,13 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "G to Source"
+                  "pathName": "G to Source Y"
                 }
               },
               {
                 "type": "named",
                 "data": {
-                  "name": "Move Elevator to 0.6 meter"
+                  "name": "Move Elevator to 0.8 meter"
                 }
               },
               {
@@ -107,7 +107,7 @@
         {
           "type": "named",
           "data": {
-            "name": "Move Elevator to 0.2 meter"
+            "name": "Move Elevator to 0.4 meter"
           }
         },
         {
@@ -117,7 +117,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Source to temp"
+                  "pathName": "Source Y to L"
                 }
               },
               {
@@ -160,6 +160,12 @@
           "data": {
             "name": "Move Arm 0 degrees"
           }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back L"
+          }
         }
       ]
     }
diff --git a/src/main/deploy/pathplanner/paths/Back G.path b/src/main/deploy/pathplanner/paths/Back G.path
index 6822cc0c..d97a36b2 100644
--- a/src/main/deploy/pathplanner/paths/Back G.path	
+++ b/src/main/deploy/pathplanner/paths/Back G.path	
@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.9,
-        "y": 3.8
+        "x": 5.86,
+        "y": 3.815
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.15,
-        "y": 3.8
+        "x": 6.131921436275931,
+        "y": 3.805199234078392
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "G Reef"
     },
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.8
+        "y": 3.815
       },
       "prevControl": {
-        "x": 5.95,
-        "y": 3.8
+        "x": 5.950621361185062,
+        "y": 3.8326151781940614
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Near G"
     }
   ],
   "rotationTargets": [],
@@ -33,9 +33,9 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
-    "maxAcceleration": 1.5,
-    "maxAngularVelocity": 1.5,
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/Source to temp.path b/src/main/deploy/pathplanner/paths/Back L.path
similarity index 70%
rename from src/main/deploy/pathplanner/paths/Source to temp.path
rename to src/main/deploy/pathplanner/paths/Back L.path
index 60879b87..c3586808 100644
--- a/src/main/deploy/pathplanner/paths/Source to temp.path	
+++ b/src/main/deploy/pathplanner/paths/Back L.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.227,
-        "y": 7.091
+        "x": 3.61,
+        "y": 5.17671875
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.3233806818181817,
-        "y": 6.094105113636363
+        "x": 3.4025986903265286,
+        "y": 5.42623827863789
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "L Reef"
     },
     {
       "anchor": {
-        "x": 3.61,
-        "y": 5.17671875
+        "x": 3.387692307692307,
+        "y": 5.480192307692307
       },
       "prevControl": {
-        "x": 2.61,
-        "y": 5.17671875
+        "x": 3.5822727940278267,
+        "y": 5.2659375102373165
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,22 +33,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
-    "maxAcceleration": 1.5,
-    "maxAngularVelocity": 1.5,
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -57.265
+    "rotation": -57.27
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -56.976000000000006
+    "rotation": -57.265
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/G to Source.path b/src/main/deploy/pathplanner/paths/G to Source Y.path
similarity index 59%
rename from src/main/deploy/pathplanner/paths/G to Source.path
rename to src/main/deploy/pathplanner/paths/G to Source Y.path
index ef7bf31d..af0dd065 100644
--- a/src/main/deploy/pathplanner/paths/G to Source.path	
+++ b/src/main/deploy/pathplanner/paths/G to Source Y.path	
@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.122556818181818,
-        "y": 3.950213068181818
+        "x": 6.2,
+        "y": 3.81
       },
       "prevControl": null,
       "nextControl": {
-        "x": 7.149630681818182,
-        "y": 5.266463068181817
+        "x": 7.109744318181819,
+        "y": 6.213792613636363
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Near G"
     },
     {
       "anchor": {
-        "x": 1.2165340909090911,
-        "y": 7.021463068181817
+        "x": 1.1567045454545453,
+        "y": 7.091264204545454
       },
       "prevControl": {
-        "x": 3.8789488636363636,
-        "y": 6.8818607954545445
+        "x": 4.0185511363636355,
+        "y": 6.562769886363637
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Source Y"
     }
   ],
   "rotationTargets": [],
@@ -33,22 +33,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
-    "maxAcceleration": 1.5,
-    "maxAngularVelocity": 1.5,
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -56.976000000000006
+    "rotation": -53.841814560191686
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -178.85423716182487
+    "rotation": 180.0
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to G.path b/src/main/deploy/pathplanner/paths/Get to G.path
index be61795e..fbab2897 100644
--- a/src/main/deploy/pathplanner/paths/Get to G.path	
+++ b/src/main/deploy/pathplanner/paths/Get to G.path	
@@ -4,28 +4,28 @@
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.8
+        "y": 3.815
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.95,
-        "y": 3.8
+        "x": 5.928238356816237,
+        "y": 3.804672387595645
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Near G"
     },
     {
       "anchor": {
-        "x": 5.9,
-        "y": 3.8
+        "x": 5.86,
+        "y": 3.815
       },
       "prevControl": {
-        "x": 6.149810079096521,
-        "y": 3.7902570855597366
+        "x": 6.109215181379802,
+        "y": 3.8347937709856583
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "G Reef"
     }
   ],
   "rotationTargets": [],
@@ -33,9 +33,9 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
-    "maxAcceleration": 1.5,
-    "maxAngularVelocity": 1.5,
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/Go Forward.path b/src/main/deploy/pathplanner/paths/Go Forward.path
index c10aacb6..e2d42621 100644
--- a/src/main/deploy/pathplanner/paths/Go Forward.path	
+++ b/src/main/deploy/pathplanner/paths/Go Forward.path	
@@ -33,9 +33,9 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
-    "maxAcceleration": 1.5,
-    "maxAngularVelocity": 1.5,
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/Source Y to L.path
similarity index 62%
rename from src/main/deploy/pathplanner/paths/New Path.path
rename to src/main/deploy/pathplanner/paths/Source Y to L.path
index 2e5404c9..28e7c231 100644
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ b/src/main/deploy/pathplanner/paths/Source Y to L.path	
@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.61,
-        "y": 5.177
+        "x": 1.1567045454545453,
+        "y": 7.091264204545454
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.609999999999999,
-        "y": 5.177
+        "x": 2.3006164708815255,
+        "y": 6.221119673625105
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Source Y"
     },
     {
       "anchor": {
-        "x": 4.0,
-        "y": 6.0
+        "x": 3.61,
+        "y": 5.17671875
       },
       "prevControl": {
-        "x": 3.0,
-        "y": 6.0
+        "x": 3.2973048614739695,
+        "y": 5.441841382452913
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "L Reef"
     }
   ],
   "rotationTargets": [],
@@ -33,22 +33,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
-    "maxAcceleration": 1.5,
-    "maxAngularVelocity": 1.5,
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": -57.265
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": -53.841814560191686
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to Near G.path b/src/main/deploy/pathplanner/paths/Start to Near G.path
index 5f00ec0a..5ff0c151 100644
--- a/src/main/deploy/pathplanner/paths/Start to Near G.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Near G.path	
@@ -8,24 +8,24 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.997,
-        "y": 3.99
+        "x": 6.970945292591921,
+        "y": 3.8734607488009942
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Center Start"
     },
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.85
+        "y": 3.815
       },
       "prevControl": {
-        "x": 7.115994318181788,
-        "y": 3.784559659090909
+        "x": 7.006960298309069,
+        "y": 3.901866007953897
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Near G"
     }
   ],
   "rotationTargets": [],
@@ -33,9 +33,9 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
-    "maxAcceleration": 1.5,
-    "maxAngularVelocity": 1.5,
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 42494837..45432d94 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -4,17 +4,17 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 1.5,
-  "defaultMaxAccel": 1.5,
-  "defaultMaxAngVel": 1.5,
+  "defaultMaxVel": 2.5,
+  "defaultMaxAccel": 2.5,
+  "defaultMaxAngVel": 90.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
   "robotMass": 74.088,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.048,
+  "driveWheelRadius": 0.0508,
   "driveGearing": 5.143,
-  "maxDriveSpeed": 5.45,
+  "maxDriveSpeed": 5.25,
   "driveMotorType": "NEO",
   "driveCurrentLimit": 60.0,
   "wheelCOF": 1.2,
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
index 29a25aec..2831a7ea 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
@@ -137,9 +137,8 @@ public RobotConfigNemo() {
       motorSettings.pid = new PIDController(0.1, 0, 0); // TODO: Tune PID controller
 
       ElevatorSettings elevatorSettings = new ElevatorSettings();
-      elevatorSettings.minHeightInMeters = 0.3;
-      elevatorSettings.maxHeightInMeters =
-          Units.inchesToMeters(74 - 18); // highest point:74 lowest point:18
+      elevatorSettings.minHeightInMeters = 0.11;
+      elevatorSettings.maxHeightInMeters = 0.02 + 0.85 + 0.76; // highest point:74 lowest point:18
       elevatorSettings.startingHeightInMeters = elevatorSettings.minHeightInMeters;
       elevatorSettings.color = new Color8Bit(Color.kSilver);
       elevatorSettings.feedforward =
@@ -203,15 +202,15 @@ public RobotConfigNemo() {
     NamedCommands.registerCommand(
         "Move Elevator to 0.5 meter", new ElevatorToPosition(elevator, () -> 0.5));
     NamedCommands.registerCommand(
-        "Move Elevator to 1.4 meter", new ElevatorToPosition(elevator, () -> 1.4));
+        "Move Elevator to 1.553 meter", new ElevatorToPosition(elevator, () -> 1.553));
     NamedCommands.registerCommand(
         "Move Arm 75 degrees", new ArmToPosition(coralArm, () -> 70).withTimeout(1.5));
     NamedCommands.registerCommand(
         "Move Arm 0 degrees", new ArmToPosition(coralArm, () -> 0).withTimeout(1.5));
     NamedCommands.registerCommand(
-        "Move Elevator to 0.6 meter", new ElevatorToPosition(elevator, () -> 0.6));
+        "Move Elevator to 0.8 meter", new ElevatorToPosition(elevator, () -> 0.8));
     NamedCommands.registerCommand(
-        "Move Elevator to 0.2 meter", new ElevatorToPosition(elevator, () -> 0.2));
+        "Move Elevator to 0.4 meter", new ElevatorToPosition(elevator, () -> 0.4));
     NamedCommands.registerCommand(
         "Move Arm for Intake", new ArmToPosition(coralArm, () -> -90).withTimeout(0));
     autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
index b7a65d58..96a23bc0 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
@@ -15,20 +15,20 @@ public static void setupController(Elevator elevator, Arm arm, CommandXboxContro
         "Driver " + "/Commands/Prepare For Intake",
         new SequentialCommandGroup(
             new ArmToPosition(arm, () -> -90).withTimeout(0),
-            new ElevatorToPosition(elevator, () -> 0.6)));
+            new ElevatorToPosition(elevator, () -> 0.8)));
 
     SmartDashboard.putData(
         "Driver " + "/Commands/Intake",
-        new SequentialCommandGroup(new ElevatorToPosition(elevator, () -> 0.2)));
+        new SequentialCommandGroup(new ElevatorToPosition(elevator, () -> 0.4)));
 
     SmartDashboard.putData(
         "Driver " + "/Commands/Prepare To Score L4",
         new SequentialCommandGroup(
             new ElevatorToPosition(elevator, () -> 1.0),
             new ArmToPosition(arm, () -> 75),
-            new ElevatorToPosition(elevator, () -> 1.4)));
+            new ElevatorToPosition(elevator, () -> 1.553)));
     SmartDashboard.putData(
         "Driver " + "/Commands/Score L4",
-        new SequentialCommandGroup(new ArmToPosition(arm, () -> -45)));
+        new SequentialCommandGroup(new ArmToPosition(arm, () -> 0)));
   }
 }

From a4aed59c1fb7517060b7f5d99f3ad6890db0d5de Mon Sep 17 00:00:00 2001
From: Carter Briscoe <tha.c.bug@gmail.com>
Date: Sat, 1 Mar 2025 14:43:19 -0500
Subject: [PATCH 11/90] DriverAssist fixes on Nemo

---
 .../controls/drive/DriveControls.java         | 87 ++++++++++++++-----
 .../drive/DriveSwerveYAGSL.java               | 10 +--
 2 files changed, 72 insertions(+), 25 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index dcc8b43b..24ffdffd 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -23,12 +23,19 @@ private enum TargetPoseOption {
     FEEDER_2(2),
     PROCESSOR(3),
     REEF_A(4),
-    REEF_G(5),
+    REEF_G(10),
     REEF_C(6),
     REEF_D(7),
-    REEF_L(8),
-    WOW_Test(9),
-    WOW_Test1(10);
+    REEF_L(15),
+    // WOW_Test(9),
+    // WOW_Test1(10),
+    REEF_B(5),
+    REEF_E(8),
+    REEF_F(9),
+    REEF_H(11),
+    REEF_I(12),
+    REEF_J(13),
+    REEF_K(14);
 
     private int index;
 
@@ -82,17 +89,26 @@ public static void setupController(Drive drive, CommandXboxController controller
         poseProcessor = new Pose2d(11.5, 7.3, Rotation2d.fromDegrees(90)),
         poseReefA = new Pose2d(15, 4.175, Rotation2d.fromDegrees(180)),
         poseReefAClose = new Pose2d(14.425, 4.175, Rotation2d.fromDegrees(180)),
-        poseReefG = new Pose2d(11, 4.175, Rotation2d.fromDegrees(0)),
+        poseReefG = new Pose2d(11.57, 4.17, Rotation2d.fromDegrees(0)),
         poseReefGClose = new Pose2d(11.5, 4.175, Rotation2d.fromDegrees(0)),
         poseReefC = new Pose2d(14.17, 5.25, Rotation2d.fromDegrees(-124)),
         poseReefD = new Pose2d(13.63, 5.56, Rotation2d.fromDegrees(-124)),
-        poseReefL = new Pose2d(14.04, 2.82, Rotation2d.fromDegrees(124)),
-        poseTest = new Pose2d(2.50, 5.57, Rotation2d.fromDegrees(-124)),
-        poseTest2 = new Pose2d(2.46, 2.73, Rotation2d.fromDegrees(-124));
+        poseReefL = new Pose2d(14.14, 2.80, Rotation2d.fromDegrees(124)),
+        // poseTest = new Pose2d(2.50, 5.57, Rotation2d.fromDegrees(-124)),
+        // poseTest2 = new Pose2d(2.46, 2.73, Rotation2d.fromDegrees(-124)),
+        poseReefB = new Pose2d(14.59, 4.34, Rotation2d.fromDegrees(180)),
+        poseReefE = new Pose2d(12.46, 5.52, Rotation2d.fromDegrees(-60)),
+        poseReefF = new Pose2d(12.00, 5.25, Rotation2d.fromDegrees(-60)),
+        poseReefH = new Pose2d(11.81, 3.92, Rotation2d.fromDegrees(-1)),
+        poseReefI = new Pose2d(12.00, 2.75, Rotation2d.fromDegrees(60)),
+        poseReefJ = new Pose2d(12.50, 2.50, Rotation2d.fromDegrees(60)),
+        poseReefK = new Pose2d(13.65, 2.50, Rotation2d.fromDegrees(120));
     //  PathConstraints constraints = new PathConstraints(4.9672, 9.3664784, 2 * Math.PI, 4 *
     //  Math.PI);
     // PathConstraints constraints = new PathConstraints(2, 1.5, 2 * Math.PI, 4 * Math.PI);
-    PathConstraints constraints = new PathConstraints(2, 1.5, Math.PI / 2, Math.PI / 4);
+    // PathConstraints constraints = new PathConstraints(4.5, 1.5, 2 * Math.PI, Math.PI / 4);
+    PathConstraints constraints = new PathConstraints(drive.getMaxLinearSpeed(), 1.5, drive.getMaxAngularSpeed(), Math.PI / 4);
+
     // PathConstraints constraints = new PathConstraints(0.5, 4.5, Math.PI / 4, 4 * Math.PI);
 
     // Temporary UI to allow user to modify destination on-the-fly
@@ -105,9 +121,16 @@ public static void setupController(Drive drive, CommandXboxController controller
     chooser.addOption("Reef G", TargetPoseOption.REEF_G);
     chooser.addOption("Reef C", TargetPoseOption.REEF_C);
     chooser.addOption("Reef D", TargetPoseOption.REEF_D);
-    chooser.addOption("Reef L", TargetPoseOption.REEF_L);
-    chooser.addOption("Test", TargetPoseOption.WOW_Test);
-    chooser.addOption("Test1", TargetPoseOption.WOW_Test1);
+    //chooser.addOption("Reef L", TargetPoseOption.REEF_L);
+    // chooser.addOption("Test", TargetPoseOption.WOW_Test);
+    // chooser.addOption("Test1", TargetPoseOption.WOW_Test1);
+    chooser.addOption("Reef B", TargetPoseOption.REEF_B);
+    chooser.addOption("Reef E", TargetPoseOption.REEF_E);
+    chooser.addOption("Reef F", TargetPoseOption.REEF_F);
+    chooser.addOption("Reef H", TargetPoseOption.REEF_H);
+    chooser.addOption("Reef I", TargetPoseOption.REEF_I);
+    chooser.addOption("Reef J", TargetPoseOption.REEF_J);
+    chooser.addOption("Reef K", TargetPoseOption.REEF_K);
     SmartDashboard.putData("Pose choices", chooser);
 
     // Define behavior for chosing destination of on-the-fly pose
@@ -122,8 +145,8 @@ public static void setupController(Drive drive, CommandXboxController controller
         .onTrue(
             new InstantCommand(
                 () -> {
-                  if (myCoolPoseKeyIdx == 8) myCoolPoseKeyIdx = 2;
-                  else myCoolPoseKeyIdx = 8;
+                  if (myCoolPoseKeyIdx == 11) myCoolPoseKeyIdx = 2;
+                  else myCoolPoseKeyIdx = 11;
                   //   if (++myCoolPoseKeyIdx == TargetPoseOption.values().length) myCoolPoseKeyIdx
                   // = 1;
                   SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
@@ -164,14 +187,38 @@ public static void setupController(Drive drive, CommandXboxController controller
                 Map.entry(
                     TargetPoseOption.REEF_L.getIndex(),
                     AutoBuilder.pathfindToPose(poseReefL, constraints, 0.0)),
+                // Map.entry(
+                //     TargetPoseOption.WOW_Test.getIndex(),
+                //     AutoBuilder.pathfindToPose(poseTest, constraints, 0.0)
+                //         .andThen(AutoBuilder.pathfindToPose(poseTest, constraints, 0.0))),
+                // Map.entry(
+                //     TargetPoseOption.WOW_Test1.getIndex(),
+                //     AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0)
+                //         .andThen(AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0))),
+                Map.entry(
+                    TargetPoseOption.REEF_B.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefB, constraints, 0.0))
+                    ,
+                Map.entry(
+                  TargetPoseOption.REEF_E.getIndex(),
+                  AutoBuilder.pathfindToPose(poseReefE, constraints, 0.0)),
+                Map.entry(
+                TargetPoseOption.REEF_F.getIndex(),
+                AutoBuilder.pathfindToPose(poseReefF, constraints, 0.0)),
                 Map.entry(
-                    TargetPoseOption.WOW_Test.getIndex(),
-                    AutoBuilder.pathfindToPose(poseTest, constraints, 0.0)
-                        .andThen(AutoBuilder.pathfindToPose(poseTest, constraints, 0.0))),
+                TargetPoseOption.REEF_H.getIndex(),
+                AutoBuilder.pathfindToPose(poseReefH, constraints, 0.0)),
                 Map.entry(
-                    TargetPoseOption.WOW_Test1.getIndex(),
-                    AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0)
-                        .andThen(AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0)))),
+                TargetPoseOption.REEF_I.getIndex(),
+                AutoBuilder.pathfindToPose(poseReefI, constraints, 0.0)),
+                Map.entry(
+                TargetPoseOption.REEF_J.getIndex(),
+                AutoBuilder.pathfindToPose(poseReefJ, constraints, 0.0)),
+                Map.entry(
+                TargetPoseOption.REEF_K.getIndex(),
+                AutoBuilder.pathfindToPose(poseReefK, constraints, 0.0))),
+  
+
             () -> {
               return myCoolPoseKeyIdx;
             });
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index 4fc75d07..778116f7 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -36,14 +36,14 @@ public static class Constants {
   private final File swerveJsonDirectory;
   private SwerveDrive swerveDrive;
   @AutoLogOutput private boolean fieldOrientedDrive = false;
-  // private PIDConstants translationPIDConstants =
-  //     new PIDConstants(5.0, 0.0, 0.0); // Translation PID constants
+   private PIDConstants translationPIDConstants =
+       new PIDConstants(7.5, 0.0, 0.0); // Translation PID constants
   // private PIDConstants rotationPIDConstants =
   //     new PIDConstants(8.0, 0.0, 1.0); // Rotation PID constants
-  private PIDConstants translationPIDConstants =
-      new PIDConstants(7.0, 0.0, 0.3); // Translation PID constants
+  // private PIDConstants translationPIDConstants =
+  //     new PIDConstants(7.0, 0.0, 0.3); // Translation PID constants
   private PIDConstants rotationPIDConstants =
-      new PIDConstants(5.0, 0.0, 0.0); // Rotation PID constants
+      new PIDConstants(6.0, 0.0, 0.0); // Rotation PID constants
 
   // @AutoLogOutput
   DriveIO io = new DriveIO();

From a1b8bdf098636038190460dfed278d04997ca24b Mon Sep 17 00:00:00 2001
From: Carter Briscoe <tha.c.bug@gmail.com>
Date: Sat, 1 Mar 2025 15:31:55 -0500
Subject: [PATCH 12/90] elevator plus drive changes

---
 .../java/frc/robot/subsystems/controls/drive/DriveControls.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 24ffdffd..e5977766 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -99,7 +99,7 @@ public static void setupController(Drive drive, CommandXboxController controller
         poseReefB = new Pose2d(14.59, 4.34, Rotation2d.fromDegrees(180)),
         poseReefE = new Pose2d(12.46, 5.52, Rotation2d.fromDegrees(-60)),
         poseReefF = new Pose2d(12.00, 5.25, Rotation2d.fromDegrees(-60)),
-        poseReefH = new Pose2d(11.81, 3.92, Rotation2d.fromDegrees(-1)),
+        poseReefH = new Pose2d(11.76, 3.92, Rotation2d.fromDegrees(-1)),
         poseReefI = new Pose2d(12.00, 2.75, Rotation2d.fromDegrees(60)),
         poseReefJ = new Pose2d(12.50, 2.50, Rotation2d.fromDegrees(60)),
         poseReefK = new Pose2d(13.65, 2.50, Rotation2d.fromDegrees(120));

From 2828b2dcce05094d111fabede0c5be41a05f2993 Mon Sep 17 00:00:00 2001
From: Carter Briscoe <tha.c.bug@gmail.com>
Date: Sat, 1 Mar 2025 16:23:51 -0500
Subject: [PATCH 13/90] Elevator and drive code fully merged

---
 .../game/reefscape2025/RobotConfig.java       |  2 +-
 .../game/reefscape2025/RobotConfigNemo.java   |  3 ++-
 .../controls/drive/DriveControls.java         | 23 ++++++++++++++++---
 3 files changed, 23 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index e15d57ab..02c5f569 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -213,7 +213,7 @@ public void configureBindings() {
     vision.setVisionMeasurementConsumer(drive::addVisionMeasurement);
 
     SendableChooser<Command> prepareScoreChooser = new SendableChooser<>();
-    DriveControls.setupController(drive, mainController);
+    DriveControls.setupController(drive, elevator, coralArm, mainController);
     DriverAssistControls.setupController(elevator, coralArm, assistController, prepareScoreChooser);
     DriverControls.setupController(elevator, coralArm, mainController, prepareScoreChooser);
     CoralArmControls.setupController(coralArm, mainController);
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
index 7941f3f1..e1ec775d 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
@@ -105,7 +105,8 @@ public RobotConfigNemo() {
       CoralArmControls.Constants.autoZeroSettings.minResetCurrent = 0.5;
       CoralArmControls.Constants.autoZeroSettings.resetPositionRad =
           Units.degreesToRadians(
-              armSettings.minAngleInDegrees); // We have an offest about 15 degrees
+              armSettings.minAngleInDegrees-10
+              ); // We have an offest about 15 degrees
       CoralArmControls.Constants.autoZeroSettings.initialReverseDuration =
           1.0; // Set the seconds of reverse before zero. Set to zero if there shound be no reverse
 
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index e5977766..f65d8f47 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -9,11 +9,18 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.drive.DriveCommand;
+import frc.robot.commands.common.elevator.ElevatorToPosition;
+import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Drive;
+import frc.robot.subsystems.interfaces.Elevator;
+
 import java.util.Map;
 
 public class DriveControls {
@@ -51,7 +58,7 @@ private TargetPoseOption(int idx) {
 
   protected static int myCoolPoseKeyIdx = 0;
 
-  public static void setupController(Drive drive, CommandXboxController controller) {
+  public static void setupController(Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
     SubsystemBase driveSubsystem = (SubsystemBase) drive;
     driveSubsystem.setDefaultCommand(
         new DriveCommand(
@@ -99,7 +106,8 @@ public static void setupController(Drive drive, CommandXboxController controller
         poseReefB = new Pose2d(14.59, 4.34, Rotation2d.fromDegrees(180)),
         poseReefE = new Pose2d(12.46, 5.52, Rotation2d.fromDegrees(-60)),
         poseReefF = new Pose2d(12.00, 5.25, Rotation2d.fromDegrees(-60)),
-        poseReefH = new Pose2d(11.76, 3.92, Rotation2d.fromDegrees(-1)),
+        poseReefHStart = new Pose2d(11.00, 3.92, Rotation2d.fromDegrees(-1)),
+        poseReefHEnd = new Pose2d(11.76, 3.92, Rotation2d.fromDegrees(-1)),
         poseReefI = new Pose2d(12.00, 2.75, Rotation2d.fromDegrees(60)),
         poseReefJ = new Pose2d(12.50, 2.50, Rotation2d.fromDegrees(60)),
         poseReefK = new Pose2d(13.65, 2.50, Rotation2d.fromDegrees(120));
@@ -207,7 +215,16 @@ public static void setupController(Drive drive, CommandXboxController controller
                 AutoBuilder.pathfindToPose(poseReefF, constraints, 0.0)),
                 Map.entry(
                 TargetPoseOption.REEF_H.getIndex(),
-                AutoBuilder.pathfindToPose(poseReefH, constraints, 0.0)),
+                AutoBuilder.pathfindToPose(poseReefHStart, constraints, 0.0)
+                  .andThen(
+                    new SequentialCommandGroup(
+                      new ElevatorToPosition(elevator, () -> 0.6),
+                      new ParallelCommandGroup(
+                          new ArmToPosition(arm, () -> 75).withTimeout(1),
+                          new ElevatorToPosition(elevator, () -> 1.553)
+                    )))
+                  .andThen(AutoBuilder.pathfindToPose(poseReefHEnd, constraints, 0.0 ))
+                  .andThen(new ArmToPosition(arm, () -> 0))),
                 Map.entry(
                 TargetPoseOption.REEF_I.getIndex(),
                 AutoBuilder.pathfindToPose(poseReefI, constraints, 0.0)),

From 9ebbac2041537a071844a71f64aeadc73c1f1731 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Sun, 2 Mar 2025 12:28:13 -0500
Subject: [PATCH 14/90] bugfix on edgecase where code crashes if all configured
 cameras are not connected

---
 .../subsystems/implementations/vision/VisionSubsystem.java      | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index 75001de8..dc3e7758 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -40,7 +40,7 @@ private class VisionCameraImpl {
     private final int index;
     private PhotonCameraSim simCamera;
     private PhotonPipelineResult result;
-    private Optional<EstimatedRobotPose> estimatedRobotPose;
+    private Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
 
     private VisionCameraImpl(Camera camera, AprilTagFieldLayout fieldLayout) {
       this.camera = new PhotonCamera(camera.getName());

From ba0d6c66a88765f530300bad46f0d4b90c9e756b Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Sun, 2 Mar 2025 13:35:37 -0500
Subject: [PATCH 15/90] spotless and TODO describing new dynamic path planning
 structure

---
 .../game/reefscape2025/RobotConfig.java       |  2 +-
 .../game/reefscape2025/RobotConfigNemo.java   |  3 +-
 .../combination/DriverAssistControls.java     | 53 +++++++-------
 .../controls/combination/DriverControls.java  | 48 +++++++------
 .../controls/drive/DriveControls.java         | 69 +++++++++++--------
 .../drive/DriveSwerveYAGSL.java               |  4 +-
 6 files changed, 94 insertions(+), 85 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index 02c5f569..5000e1d5 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -127,7 +127,7 @@ public RobotConfig(
 
       ElevatorSettings elevatorSettings = new ElevatorSettings();
       elevatorSettings.minHeightInMeters = 0.0;
-      elevatorSettings.maxHeightInMeters = 0.02 + 0.85 + 0.76;;
+      elevatorSettings.maxHeightInMeters = 0.02 + 0.85 + 0.76;
       elevatorSettings.startingHeightInMeters = elevatorSettings.minHeightInMeters;
       elevatorSettings.targetHeightToleranceInMeters = 0.01;
       elevatorSettings.color = new Color8Bit(Color.kSilver);
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
index e1ec775d..5271b19b 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
@@ -105,8 +105,7 @@ public RobotConfigNemo() {
       CoralArmControls.Constants.autoZeroSettings.minResetCurrent = 0.5;
       CoralArmControls.Constants.autoZeroSettings.resetPositionRad =
           Units.degreesToRadians(
-              armSettings.minAngleInDegrees-10
-              ); // We have an offest about 15 degrees
+              armSettings.minAngleInDegrees - 10); // We have an offest about 15 degrees
       CoralArmControls.Constants.autoZeroSettings.initialReverseDuration =
           1.0; // Set the seconds of reverse before zero. Set to zero if there shound be no reverse
 
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
index 0ef37843..098664ac 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
@@ -1,11 +1,8 @@
 package frc.robot.subsystems.controls.combination;
 
-import java.util.function.Consumer;
-
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -15,7 +12,11 @@
 import frc.robot.subsystems.interfaces.Elevator;
 
 public class DriverAssistControls {
-  public static void setupController(Elevator elevator, Arm arm, CommandXboxController controller, SendableChooser<Command> prepareScoreChooser) {
+  public static void setupController(
+      Elevator elevator,
+      Arm arm,
+      CommandXboxController controller,
+      SendableChooser<Command> prepareScoreChooser) {
     SmartDashboard.putData(
         "Driver " + "/Commands/Prepare For Intake",
         new SequentialCommandGroup(
@@ -26,36 +27,32 @@ public static void setupController(Elevator elevator, Arm arm, CommandXboxContro
         "Driver " + "/Commands/Intake",
         new SequentialCommandGroup(new ElevatorToPosition(elevator, () -> 0.4)));
 
-
-    prepareScoreChooser.setDefaultOption("L2", 
+    prepareScoreChooser.setDefaultOption(
+        "L2",
         new SequentialCommandGroup(
-                new ElevatorToPosition(elevator, () -> 0.63),
-                new ArmToPosition(arm, () -> 75)));
+            new ElevatorToPosition(elevator, () -> 0.63), new ArmToPosition(arm, () -> 75)));
 
-    prepareScoreChooser.addOption("L3", 
+    prepareScoreChooser.addOption(
+        "L3",
         new SequentialCommandGroup(
-                new ElevatorToPosition(elevator, () -> 1.0),
-                new ArmToPosition(arm, () -> 75)));
+            new ElevatorToPosition(elevator, () -> 1.0), new ArmToPosition(arm, () -> 75)));
 
-    prepareScoreChooser.addOption("L4", 
+    prepareScoreChooser.addOption(
+        "L4",
         new SequentialCommandGroup(
-        new ElevatorToPosition(elevator, () -> 0.6),
-        new ParallelCommandGroup(
-            new ArmToPosition(arm, () -> 75),
-            new ElevatorToPosition(elevator, () -> 1.553)
-        )));
-            SmartDashboard.putData("Driver " + "/Commands/Prepare To Score Command", prepareScoreChooser.getSelected());
-    prepareScoreChooser.onChange((selected) -> {
-        SmartDashboard.putData("Driver " + "/Commands/Prepare To Score Command", selected);
-    });
+            new ElevatorToPosition(elevator, () -> 0.6),
+            new ParallelCommandGroup(
+                new ArmToPosition(arm, () -> 75), new ElevatorToPosition(elevator, () -> 1.553))));
     SmartDashboard.putData(
-        "Driver " + "/Commands/Prepare To Score Chooser",
-        prepareScoreChooser);
-            
-      
+        "Driver " + "/Commands/Prepare To Score Command", prepareScoreChooser.getSelected());
+    prepareScoreChooser.onChange(
+        (selected) -> {
+          SmartDashboard.putData("Driver " + "/Commands/Prepare To Score Command", selected);
+        });
+    SmartDashboard.putData("Driver " + "/Commands/Prepare To Score Chooser", prepareScoreChooser);
+
     SmartDashboard.putData(
-        "Driver " + "/Commands/Score",
-        new SequentialCommandGroup(new ArmToPosition(arm, () -> 0)));
+        "Driver " + "/Commands/Score", new SequentialCommandGroup(new ArmToPosition(arm, () -> 0)));
 
     SmartDashboard.putData(
         "Driver " + "/Commands/Prepare To Remove Algae L2",
@@ -71,6 +68,4 @@ public static void setupController(Elevator elevator, Arm arm, CommandXboxContro
             new ArmToPosition(arm, () -> 0),
             new ElevatorToPosition(elevator, () -> 1.3)));
   }
-
-  
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 7b917d6e..5f66901a 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -1,13 +1,7 @@
 package frc.robot.subsystems.controls.combination;
 
-import java.util.function.BooleanSupplier;
-import java.util.function.Consumer;
-
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -17,25 +11,37 @@
 import frc.robot.subsystems.interfaces.Elevator;
 
 public class DriverControls {
-  public static void setupController(Elevator elevator, Arm arm, CommandXboxController controller, SendableChooser<Command> prepareScoreChooser) {
-    Trigger ableToIntake =  new Trigger(() -> {
-        if(elevator.getCurrentHeight() > 0.5) {
-            return true;
-        }
-        return false;
-    });
-    controller.a().and(ableToIntake).onTrue(new SequentialCommandGroup(
-        new ArmToPosition(arm, () -> -90).withTimeout(0),
-        new ElevatorToPosition(elevator, () -> 0.8)));
+  public static void setupController(
+      Elevator elevator,
+      Arm arm,
+      CommandXboxController controller,
+      SendableChooser<Command> prepareScoreChooser) {
+    Trigger ableToIntake =
+        new Trigger(
+            () -> {
+              if (elevator.getCurrentHeight() > 0.5) {
+                return true;
+              }
+              return false;
+            });
+    controller
+        .a()
+        .and(ableToIntake)
+        .onTrue(
+            new SequentialCommandGroup(
+                new ArmToPosition(arm, () -> -90).withTimeout(0),
+                new ElevatorToPosition(elevator, () -> 0.8)));
 
-    controller. leftTrigger().onTrue(new SequentialCommandGroup(new ElevatorToPosition(elevator, () -> 0.3)));
+    controller
+        .leftTrigger()
+        .onTrue(new SequentialCommandGroup(new ElevatorToPosition(elevator, () -> 0.3)));
 
     controller.rightTrigger().onTrue(new SequentialCommandGroup(new ArmToPosition(arm, () -> 0)));
 
     controller.y().onTrue(prepareScoreChooser.getSelected());
-    prepareScoreChooser.onChange((selected) -> {
-        controller.y().onTrue(prepareScoreChooser.getSelected());
-    });
+    prepareScoreChooser.onChange(
+        (selected) -> {
+          controller.y().onTrue(prepareScoreChooser.getSelected());
+        });
   }
-  
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index f65d8f47..3392e816 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -20,7 +20,6 @@
 import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Drive;
 import frc.robot.subsystems.interfaces.Elevator;
-
 import java.util.Map;
 
 public class DriveControls {
@@ -56,9 +55,10 @@ private TargetPoseOption(int idx) {
     }
   }
 
-  protected static int myCoolPoseKeyIdx = 0;
+  protected static int myCoolPoseKeyIdx = 11;
 
-  public static void setupController(Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
+  public static void setupController(
+      Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
     SubsystemBase driveSubsystem = (SubsystemBase) drive;
     driveSubsystem.setDefaultCommand(
         new DriveCommand(
@@ -115,7 +115,9 @@ public static void setupController(Drive drive, Elevator elevator, Arm arm, Comm
     //  Math.PI);
     // PathConstraints constraints = new PathConstraints(2, 1.5, 2 * Math.PI, 4 * Math.PI);
     // PathConstraints constraints = new PathConstraints(4.5, 1.5, 2 * Math.PI, Math.PI / 4);
-    PathConstraints constraints = new PathConstraints(drive.getMaxLinearSpeed(), 1.5, drive.getMaxAngularSpeed(), Math.PI / 4);
+    PathConstraints constraints =
+        new PathConstraints(
+            drive.getMaxLinearSpeed(), 1.5, drive.getMaxAngularSpeed(), Math.PI / 4);
 
     // PathConstraints constraints = new PathConstraints(0.5, 4.5, Math.PI / 4, 4 * Math.PI);
 
@@ -129,7 +131,7 @@ public static void setupController(Drive drive, Elevator elevator, Arm arm, Comm
     chooser.addOption("Reef G", TargetPoseOption.REEF_G);
     chooser.addOption("Reef C", TargetPoseOption.REEF_C);
     chooser.addOption("Reef D", TargetPoseOption.REEF_D);
-    //chooser.addOption("Reef L", TargetPoseOption.REEF_L);
+    // chooser.addOption("Reef L", TargetPoseOption.REEF_L);
     // chooser.addOption("Test", TargetPoseOption.WOW_Test);
     // chooser.addOption("Test1", TargetPoseOption.WOW_Test1);
     chooser.addOption("Reef B", TargetPoseOption.REEF_B);
@@ -205,42 +207,49 @@ public static void setupController(Drive drive, Elevator elevator, Arm arm, Comm
                 //         .andThen(AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0))),
                 Map.entry(
                     TargetPoseOption.REEF_B.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefB, constraints, 0.0))
-                    ,
+                    AutoBuilder.pathfindToPose(poseReefB, constraints, 0.0)),
                 Map.entry(
-                  TargetPoseOption.REEF_E.getIndex(),
-                  AutoBuilder.pathfindToPose(poseReefE, constraints, 0.0)),
+                    TargetPoseOption.REEF_E.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefE, constraints, 0.0)),
                 Map.entry(
-                TargetPoseOption.REEF_F.getIndex(),
-                AutoBuilder.pathfindToPose(poseReefF, constraints, 0.0)),
+                    TargetPoseOption.REEF_F.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefF, constraints, 0.0)),
                 Map.entry(
-                TargetPoseOption.REEF_H.getIndex(),
-                AutoBuilder.pathfindToPose(poseReefHStart, constraints, 0.0)
-                  .andThen(
-                    new SequentialCommandGroup(
-                      new ElevatorToPosition(elevator, () -> 0.6),
-                      new ParallelCommandGroup(
-                          new ArmToPosition(arm, () -> 75).withTimeout(1),
-                          new ElevatorToPosition(elevator, () -> 1.553)
-                    )))
-                  .andThen(AutoBuilder.pathfindToPose(poseReefHEnd, constraints, 0.0 ))
-                  .andThen(new ArmToPosition(arm, () -> 0))),
+                    TargetPoseOption.REEF_H.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefHStart, constraints, 0.0)
+                        .andThen(
+                            new SequentialCommandGroup(
+                                new ElevatorToPosition(elevator, () -> 0.6),
+                                new ParallelCommandGroup(
+                                    new ArmToPosition(arm, () -> 75).withTimeout(1),
+                                    new ElevatorToPosition(elevator, () -> 1.553))))
+                        .andThen(AutoBuilder.pathfindToPose(poseReefHEnd, constraints, 0.0))
+                        .andThen(new ArmToPosition(arm, () -> 0))),
                 Map.entry(
-                TargetPoseOption.REEF_I.getIndex(),
-                AutoBuilder.pathfindToPose(poseReefI, constraints, 0.0)),
+                    TargetPoseOption.REEF_I.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefI, constraints, 0.0)),
                 Map.entry(
-                TargetPoseOption.REEF_J.getIndex(),
-                AutoBuilder.pathfindToPose(poseReefJ, constraints, 0.0)),
+                    TargetPoseOption.REEF_J.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefJ, constraints, 0.0)),
                 Map.entry(
-                TargetPoseOption.REEF_K.getIndex(),
-                AutoBuilder.pathfindToPose(poseReefK, constraints, 0.0))),
-  
-
+                    TargetPoseOption.REEF_K.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefK, constraints, 0.0))),
             () -> {
               return myCoolPoseKeyIdx;
             });
     // ), () -> { return chooser.getSelected(); });
 
+    // TODO: new dynamic path planning command creation
+    // 1. SelectCommand ONE: go to initial location (based on user-chosen option X)
+    // 2. SelectCommand TWO: determine next action (based on option X and potentially pre-decided
+    // reef elevation value)
+    // 2a. if (1) was a reef position, assume we are scoring --> get reef elevation to score on -->
+    // set elevator/arm
+    // 2b. if not, choose an empty command and short-circuit (exit) the sequence
+    // 3. SelectCommand THREE: go to scoring position (which maps directly from initial chosen
+    // location)
+    // 4. Execute <arm down command>
+
     // dynamically go to destination
     controller.b().whileTrue(coolGoToPose);
   }
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index 778116f7..2df7da0a 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -36,8 +36,8 @@ public static class Constants {
   private final File swerveJsonDirectory;
   private SwerveDrive swerveDrive;
   @AutoLogOutput private boolean fieldOrientedDrive = false;
-   private PIDConstants translationPIDConstants =
-       new PIDConstants(7.5, 0.0, 0.0); // Translation PID constants
+  private PIDConstants translationPIDConstants =
+      new PIDConstants(7.5, 0.0, 0.0); // Translation PID constants
   // private PIDConstants rotationPIDConstants =
   //     new PIDConstants(8.0, 0.0, 1.0); // Rotation PID constants
   // private PIDConstants translationPIDConstants =

From dad9c6fef05bff59bb11ff754392b5c98b9e87a8 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Mon, 3 Mar 2025 18:49:14 -0500
Subject: [PATCH 16/90] Pulled from driverCommands and updated paths based on
 comp bot

---
 src/main/deploy/pathplanner/paths/Back G.path | 12 ++--
 .../pathplanner/paths/G to Source Y.path      |  4 +-
 .../deploy/pathplanner/paths/Get to G.path    | 12 ++--
 .../pathplanner/paths/Start to Near G.path    |  4 +-
 .../game/reefscape2025/RobotConfig.java       |  4 --
 .../game/reefscape2025/RobotConfigComp.java   | 36 +++++++++-
 .../game/reefscape2025/RobotConfigNemo.java   |  3 +-
 .../controls/combination/DriverControls.java  |  7 +-
 .../controls/drive/DriveControls.java         | 68 +++++++++----------
 .../drive/DriveSwerveYAGSL.java               |  4 +-
 .../vision/VisionSubsystem.java               |  2 +-
 11 files changed, 92 insertions(+), 64 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/Back G.path b/src/main/deploy/pathplanner/paths/Back G.path
index d97a36b2..36af730b 100644
--- a/src/main/deploy/pathplanner/paths/Back G.path	
+++ b/src/main/deploy/pathplanner/paths/Back G.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.86,
-        "y": 3.815
+        "x": 5.87,
+        "y": 3.85
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.131921436275931,
-        "y": 3.805199234078392
+        "x": 6.141921436275931,
+        "y": 3.840199234078392
       },
       "isLocked": false,
       "linkedName": "G Reef"
@@ -17,11 +17,11 @@
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.815
+        "y": 3.85
       },
       "prevControl": {
         "x": 5.950621361185062,
-        "y": 3.8326151781940614
+        "y": 3.8676151781940615
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/G to Source Y.path b/src/main/deploy/pathplanner/paths/G to Source Y.path
index af0dd065..436c7bc7 100644
--- a/src/main/deploy/pathplanner/paths/G to Source Y.path	
+++ b/src/main/deploy/pathplanner/paths/G to Source Y.path	
@@ -4,12 +4,12 @@
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.81
+        "y": 3.85
       },
       "prevControl": null,
       "nextControl": {
         "x": 7.109744318181819,
-        "y": 6.213792613636363
+        "y": 6.253792613636363
       },
       "isLocked": false,
       "linkedName": "Near G"
diff --git a/src/main/deploy/pathplanner/paths/Get to G.path b/src/main/deploy/pathplanner/paths/Get to G.path
index fbab2897..19024cc8 100644
--- a/src/main/deploy/pathplanner/paths/Get to G.path	
+++ b/src/main/deploy/pathplanner/paths/Get to G.path	
@@ -4,24 +4,24 @@
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.815
+        "y": 3.85
       },
       "prevControl": null,
       "nextControl": {
         "x": 5.928238356816237,
-        "y": 3.804672387595645
+        "y": 3.839672387595645
       },
       "isLocked": false,
       "linkedName": "Near G"
     },
     {
       "anchor": {
-        "x": 5.86,
-        "y": 3.815
+        "x": 5.87,
+        "y": 3.85
       },
       "prevControl": {
-        "x": 6.109215181379802,
-        "y": 3.8347937709856583
+        "x": 6.1192151813798015,
+        "y": 3.8697937709856585
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Start to Near G.path b/src/main/deploy/pathplanner/paths/Start to Near G.path
index 5ff0c151..6519c3b1 100644
--- a/src/main/deploy/pathplanner/paths/Start to Near G.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Near G.path	
@@ -17,11 +17,11 @@
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.815
+        "y": 3.85
       },
       "prevControl": {
         "x": 7.006960298309069,
-        "y": 3.901866007953897
+        "y": 3.9368660079538973
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index f6d50f44..79f3b89f 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -212,13 +212,9 @@ public void configureBindings() {
     // Send vision-based odometry measurements to drive's odometry calculations
     vision.setVisionMeasurementConsumer(drive::addVisionMeasurement);
 
-    DriveControls.setupController(drive, mainController);
     DriverAssistControls.setupController(elevator, coralArm, assistController);
     DriverControls.setupController(elevator, coralArm, mainController);
-    SendableChooser<Command> prepareScoreChooser = new SendableChooser<>();
     DriveControls.setupController(drive, elevator, coralArm, mainController);
-    DriverAssistControls.setupController(elevator, coralArm, assistController, prepareScoreChooser);
-    DriverControls.setupController(elevator, coralArm, mainController, prepareScoreChooser);
     CoralArmControls.setupController(coralArm, mainController);
     ElevatorControls.setupController(elevator, mainController);
     ClimberArmControls.setupController(climberArm, mainController);
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 2e65db5b..be7285d9 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -1,12 +1,19 @@
 package frc.robot.config.game.reefscape2025;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
+import frc.robot.commands.common.arm.ArmToPosition;
+import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.io.implementations.motor.MotorIOBase.MotorIOBaseSettings;
 import frc.robot.io.implementations.motor.MotorIOTalonFx;
 import frc.robot.io.implementations.motor.MotorIOTalonFx.TalonFxSettings;
@@ -19,11 +26,21 @@
 import frc.robot.subsystems.interfaces.Arm.ArmSettings;
 import frc.robot.subsystems.interfaces.Drive;
 import frc.robot.subsystems.interfaces.Elevator.ElevatorSettings;
+import frc.robot.subsystems.interfaces.Vision.Camera;
 
 public class RobotConfigComp extends RobotConfig {
   public RobotConfigComp() {
-    super(false, true, true, false, false, true, true);
-
+    super(false, false, false, false, false, true, true);
+
+    vision.addCamera(
+        new Camera(
+            "front_cam", // back
+            new Transform3d(
+                new Translation3d(
+                    Units.inchesToMeters(14.25),
+                    Units.inchesToMeters(2.5),
+                    Units.inchesToMeters(7.125)),
+                new Rotation3d(0.0, 0.0, 0.0))));
     // Comp has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
     Drive.Constants.rotatePidKi = 0.0;
@@ -153,5 +170,20 @@ public RobotConfigComp() {
     //             // new MotorIOArmStub(motorSettings, armSettings), "Coral", armSettings);
     //             new MotorIOTalonFx(motorSettings, settings), "Climber", armSettings);
     // }
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.5 meter", new ElevatorToPosition(elevator, () -> 0.5));
+    NamedCommands.registerCommand(
+        "Move Elevator to 1.553 meter", new ElevatorToPosition(elevator, () -> 1.553));
+    NamedCommands.registerCommand(
+        "Move Arm 75 degrees", new ArmToPosition(coralArm, () -> 70).withTimeout(1.5));
+    NamedCommands.registerCommand(
+        "Move Arm 0 degrees", new ArmToPosition(coralArm, () -> 0).withTimeout(1.5));
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.8 meter", new ElevatorToPosition(elevator, () -> 0.8));
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.4 meter", new ElevatorToPosition(elevator, () -> 0.4));
+    NamedCommands.registerCommand(
+        "Move Arm for Intake", new ArmToPosition(coralArm, () -> -90).withTimeout(0));
+    autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
   }
 }
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
index 65b7c959..e5b738f2 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
@@ -109,8 +109,7 @@ public RobotConfigNemo() {
       CoralArmControls.Constants.autoZeroSettings.minResetCurrent = 0.5;
       CoralArmControls.Constants.autoZeroSettings.resetPositionRad =
           Units.degreesToRadians(
-              armSettings.minAngleInDegrees-10
-              ); // We have an offest about 15 degrees
+              armSettings.minAngleInDegrees - 10); // We have an offest about 15 degrees
       CoralArmControls.Constants.autoZeroSettings.initialReverseDuration =
           1.0; // Set the seconds of reverse before zero. Set to zero if there shound be no reverse
 
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index c8485447..1cdcb26b 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -57,9 +57,12 @@ public static void setupController(Elevator elevator, Arm arm, CommandXboxContro
                 new ArmToPosition(arm, () -> 75), new ElevatorToPosition(elevator, () -> 1.553))));
 
     SmartDashboard.putData(
-        "Driver " + "/Commands/Prepare To Score Command", DriverControls.Choosers.prepareScoreChooser.getSelected());
+        "Driver " + "/Commands/Prepare To Score Command",
+        DriverControls.Choosers.prepareScoreChooser.getSelected());
 
-    SmartDashboard.putData("Driver " + "/Commands/Prepare To Score Chooser", DriverControls.Choosers.prepareScoreChooser);
+    SmartDashboard.putData(
+        "Driver " + "/Commands/Prepare To Score Chooser",
+        DriverControls.Choosers.prepareScoreChooser);
 
     controller.y().onTrue(DriverControls.Choosers.prepareScoreChooser.getSelected());
     DriverControls.Choosers.prepareScoreChooser.onChange(
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index f65d8f47..41ebbb00 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -20,7 +20,6 @@
 import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Drive;
 import frc.robot.subsystems.interfaces.Elevator;
-
 import java.util.Map;
 
 public class DriveControls {
@@ -58,7 +57,8 @@ private TargetPoseOption(int idx) {
 
   protected static int myCoolPoseKeyIdx = 0;
 
-  public static void setupController(Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
+  public static void setupController(
+      Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
     SubsystemBase driveSubsystem = (SubsystemBase) drive;
     driveSubsystem.setDefaultCommand(
         new DriveCommand(
@@ -115,7 +115,9 @@ public static void setupController(Drive drive, Elevator elevator, Arm arm, Comm
     //  Math.PI);
     // PathConstraints constraints = new PathConstraints(2, 1.5, 2 * Math.PI, 4 * Math.PI);
     // PathConstraints constraints = new PathConstraints(4.5, 1.5, 2 * Math.PI, Math.PI / 4);
-    PathConstraints constraints = new PathConstraints(drive.getMaxLinearSpeed(), 1.5, drive.getMaxAngularSpeed(), Math.PI / 4);
+    PathConstraints constraints =
+        new PathConstraints(
+            drive.getMaxLinearSpeed(), 1.5, drive.getMaxAngularSpeed(), Math.PI / 4);
 
     // PathConstraints constraints = new PathConstraints(0.5, 4.5, Math.PI / 4, 4 * Math.PI);
 
@@ -129,7 +131,7 @@ public static void setupController(Drive drive, Elevator elevator, Arm arm, Comm
     chooser.addOption("Reef G", TargetPoseOption.REEF_G);
     chooser.addOption("Reef C", TargetPoseOption.REEF_C);
     chooser.addOption("Reef D", TargetPoseOption.REEF_D);
-    //chooser.addOption("Reef L", TargetPoseOption.REEF_L);
+    // chooser.addOption("Reef L", TargetPoseOption.REEF_L);
     // chooser.addOption("Test", TargetPoseOption.WOW_Test);
     // chooser.addOption("Test1", TargetPoseOption.WOW_Test1);
     chooser.addOption("Reef B", TargetPoseOption.REEF_B);
@@ -205,37 +207,33 @@ public static void setupController(Drive drive, Elevator elevator, Arm arm, Comm
                 //         .andThen(AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0))),
                 Map.entry(
                     TargetPoseOption.REEF_B.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefB, constraints, 0.0))
-                    ,
-                Map.entry(
-                  TargetPoseOption.REEF_E.getIndex(),
-                  AutoBuilder.pathfindToPose(poseReefE, constraints, 0.0)),
-                Map.entry(
-                TargetPoseOption.REEF_F.getIndex(),
-                AutoBuilder.pathfindToPose(poseReefF, constraints, 0.0)),
-                Map.entry(
-                TargetPoseOption.REEF_H.getIndex(),
-                AutoBuilder.pathfindToPose(poseReefHStart, constraints, 0.0)
-                  .andThen(
-                    new SequentialCommandGroup(
-                      new ElevatorToPosition(elevator, () -> 0.6),
-                      new ParallelCommandGroup(
-                          new ArmToPosition(arm, () -> 75).withTimeout(1),
-                          new ElevatorToPosition(elevator, () -> 1.553)
-                    )))
-                  .andThen(AutoBuilder.pathfindToPose(poseReefHEnd, constraints, 0.0 ))
-                  .andThen(new ArmToPosition(arm, () -> 0))),
-                Map.entry(
-                TargetPoseOption.REEF_I.getIndex(),
-                AutoBuilder.pathfindToPose(poseReefI, constraints, 0.0)),
-                Map.entry(
-                TargetPoseOption.REEF_J.getIndex(),
-                AutoBuilder.pathfindToPose(poseReefJ, constraints, 0.0)),
-                Map.entry(
-                TargetPoseOption.REEF_K.getIndex(),
-                AutoBuilder.pathfindToPose(poseReefK, constraints, 0.0))),
-  
-
+                    AutoBuilder.pathfindToPose(poseReefB, constraints, 0.0)),
+                Map.entry(
+                    TargetPoseOption.REEF_E.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefE, constraints, 0.0)),
+                Map.entry(
+                    TargetPoseOption.REEF_F.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefF, constraints, 0.0)),
+                Map.entry(
+                    TargetPoseOption.REEF_H.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefHStart, constraints, 0.0)
+                        .andThen(
+                            new SequentialCommandGroup(
+                                new ElevatorToPosition(elevator, () -> 0.6),
+                                new ParallelCommandGroup(
+                                    new ArmToPosition(arm, () -> 75).withTimeout(1),
+                                    new ElevatorToPosition(elevator, () -> 1.553))))
+                        .andThen(AutoBuilder.pathfindToPose(poseReefHEnd, constraints, 0.0))
+                        .andThen(new ArmToPosition(arm, () -> 0))),
+                Map.entry(
+                    TargetPoseOption.REEF_I.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefI, constraints, 0.0)),
+                Map.entry(
+                    TargetPoseOption.REEF_J.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefJ, constraints, 0.0)),
+                Map.entry(
+                    TargetPoseOption.REEF_K.getIndex(),
+                    AutoBuilder.pathfindToPose(poseReefK, constraints, 0.0))),
             () -> {
               return myCoolPoseKeyIdx;
             });
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index 778116f7..2df7da0a 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -36,8 +36,8 @@ public static class Constants {
   private final File swerveJsonDirectory;
   private SwerveDrive swerveDrive;
   @AutoLogOutput private boolean fieldOrientedDrive = false;
-   private PIDConstants translationPIDConstants =
-       new PIDConstants(7.5, 0.0, 0.0); // Translation PID constants
+  private PIDConstants translationPIDConstants =
+      new PIDConstants(7.5, 0.0, 0.0); // Translation PID constants
   // private PIDConstants rotationPIDConstants =
   //     new PIDConstants(8.0, 0.0, 1.0); // Rotation PID constants
   // private PIDConstants translationPIDConstants =
diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index 75001de8..e4ec9385 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -178,7 +178,7 @@ public void updatePoseEstimation() {
         debugTargetDistance = camera.getDistanceToBestTarget();
 
         // Add vision measurement to the consumer.
-        if (visionMeasurementConsumer != null && debugTargetDistance < 0.8) {
+        if (visionMeasurementConsumer != null && debugTargetDistance < 5) {
           visionMeasurementConsumer.add(
               currentEstimatedRobotPose.get().estimatedPose.toPose2d(),
               currentEstimatedRobotPose.get().timestampSeconds,

From 0521de5e0508bd0b7a90c942c96bde9c74a07705 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Mon, 3 Mar 2025 20:16:07 -0500
Subject: [PATCH 17/90] lower current limit

---
 .../frc/robot/io/implementations/motor/MotorIOSparkMax.java     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/io/implementations/motor/MotorIOSparkMax.java b/src/main/java/frc/robot/io/implementations/motor/MotorIOSparkMax.java
index 3aa72b4d..dfd3aad9 100644
--- a/src/main/java/frc/robot/io/implementations/motor/MotorIOSparkMax.java
+++ b/src/main/java/frc/robot/io/implementations/motor/MotorIOSparkMax.java
@@ -36,7 +36,7 @@ public MotorIOSparkMax(MotorIOBaseSettings motorSettings, SparkMaxSettings spark
 
     motorConfig
         .inverted(motorSettings.motor.inverted)
-        .smartCurrentLimit(60, 40)
+        .smartCurrentLimit(45, 30)
         .secondaryCurrentLimit(0)
         .idleMode(SparkBaseConfig.IdleMode.kBrake);
 

From 043a394876ea85f9bf2b0efb389b0aade398c014 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Mon, 3 Mar 2025 20:16:15 -0500
Subject: [PATCH 18/90] use chooser

---
 src/main/deploy/pathplanner/settings.json     |  4 +-
 .../game/reefscape2025/RobotConfig.java       |  2 +-
 .../controls/drive/DriveControls.java         | 56 +++++++++++++++----
 3 files changed, 48 insertions(+), 14 deletions(-)

diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 1aa11407..d5d1361d 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -1,6 +1,6 @@
 {
-  "robotWidth": 0.711,
-  "robotLength": 0.7112,
+  "robotWidth": 0.8,
+  "robotLength": 0.8,
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index adf04399..79f3b89f 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -212,9 +212,9 @@ public void configureBindings() {
     // Send vision-based odometry measurements to drive's odometry calculations
     vision.setVisionMeasurementConsumer(drive::addVisionMeasurement);
 
-    DriveControls.setupController(drive, elevator, coralArm, mainController);
     DriverAssistControls.setupController(elevator, coralArm, assistController);
     DriverControls.setupController(elevator, coralArm, mainController);
+    DriveControls.setupController(drive, elevator, coralArm, mainController);
     CoralArmControls.setupController(coralArm, mainController);
     ElevatorControls.setupController(elevator, mainController);
     ClimberArmControls.setupController(climberArm, mainController);
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 3392e816..04c055e1 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.ProxyCommand;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -17,10 +18,12 @@
 import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.drive.DriveCommand;
 import frc.robot.commands.common.elevator.ElevatorToPosition;
+import frc.robot.subsystems.controls.combination.DriverControls;
 import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Drive;
 import frc.robot.subsystems.interfaces.Elevator;
 import java.util.Map;
+import java.util.Map.Entry;
 
 public class DriveControls {
   private enum TargetPoseOption {
@@ -214,17 +217,25 @@ public static void setupController(
                 Map.entry(
                     TargetPoseOption.REEF_F.getIndex(),
                     AutoBuilder.pathfindToPose(poseReefF, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.REEF_H.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefHStart, constraints, 0.0)
-                        .andThen(
-                            new SequentialCommandGroup(
-                                new ElevatorToPosition(elevator, () -> 0.6),
-                                new ParallelCommandGroup(
-                                    new ArmToPosition(arm, () -> 75).withTimeout(1),
-                                    new ElevatorToPosition(elevator, () -> 1.553))))
-                        .andThen(AutoBuilder.pathfindToPose(poseReefHEnd, constraints, 0.0))
-                        .andThen(new ArmToPosition(arm, () -> 0))),
+                createDynamicPathScoringCommand(
+                  TargetPoseOption.REEF_H.getIndex(),
+                  poseReefHStart,
+                  poseReefHEnd,
+                  constraints,
+                  DriverControls.Constants.prepareScoreCommand,
+                  new ArmToPosition(arm, () -> 0)
+                ),
+                // Map.entry(
+                //     TargetPoseOption.REEF_H.getIndex(),
+                //     AutoBuilder.pathfindToPose(poseReefHStart, constraints, 0.0)
+                //         .andThen(
+                //             new SequentialCommandGroup(
+                //                 new ElevatorToPosition(elevator, () -> 0.6),
+                //                 new ParallelCommandGroup(
+                //                     new ArmToPosition(arm, () -> 75).withTimeout(1),
+                //                     new ElevatorToPosition(elevator, () -> 1.553))))
+                //         .andThen(AutoBuilder.pathfindToPose(poseReefHEnd, constraints, 0.0))
+                //         .andThen(new ArmToPosition(arm, () -> 0))),
                 Map.entry(
                     TargetPoseOption.REEF_I.getIndex(),
                     AutoBuilder.pathfindToPose(poseReefI, constraints, 0.0)),
@@ -253,4 +264,27 @@ public static void setupController(
     // dynamically go to destination
     controller.b().whileTrue(coolGoToPose);
   }
+
+  private static Entry<Integer, Command> createDynamicPathCommand(int index, Pose2d endingPose, PathConstraints constraints) {
+    Entry<Integer, Command> entry = 
+      Map.entry(
+        index,
+          AutoBuilder.pathfindToPose(endingPose, constraints, 0.0)
+      );
+    return null;
+  }
+  private static Entry<Integer, Command> createDynamicPathScoringCommand(int index, Pose2d initialPose, Pose2d scoringPose, PathConstraints constraints, Command prepareToScoreCommand, Command scoreCommand) {
+    Entry<Integer, Command> entry = 
+      Map.entry(
+        index,
+        new SequentialCommandGroup(
+          AutoBuilder.pathfindToPose(initialPose, constraints, 0.0),
+          prepareToScoreCommand,
+          AutoBuilder.pathfindToPose(scoringPose, constraints, 0.0),
+          scoreCommand
+        )
+      );
+      return entry;
+    
+  }
 }

From a4b6a1435cbfb33317943a0bcc7601de6e2bc0f0 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Mon, 3 Mar 2025 21:05:18 -0500
Subject: [PATCH 19/90] Added side auto. need to test

---
 ...L L4 .auto => Score I L4 + Y + L L4 .auto} | 12 +--
 .../deploy/pathplanner/autos/Score I L4.auto  | 76 +++++++++++++++++++
 src/main/deploy/pathplanner/paths/Back I.path | 54 +++++++++++++
 .../pathplanner/paths/I to Source Y.path      | 54 +++++++++++++
 .../deploy/pathplanner/paths/Start to I.path  | 54 +++++++++++++
 .../game/reefscape2025/RobotConfigComp.java   | 21 ++++-
 .../vision/VisionSubsystem.java               | 10 ++-
 7 files changed, 269 insertions(+), 12 deletions(-)
 rename src/main/deploy/pathplanner/autos/{Score G L4 + Y + L L4 .auto => Score I L4 + Y + L L4 .auto} (94%)
 create mode 100644 src/main/deploy/pathplanner/autos/Score I L4.auto
 create mode 100644 src/main/deploy/pathplanner/paths/Back I.path
 create mode 100644 src/main/deploy/pathplanner/paths/I to Source Y.path
 create mode 100644 src/main/deploy/pathplanner/paths/Start to I.path

diff --git a/src/main/deploy/pathplanner/autos/Score G L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto
similarity index 94%
rename from src/main/deploy/pathplanner/autos/Score G L4 + Y + L L4 .auto
rename to src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto
index 169cbafa..bcb732f3 100644
--- a/src/main/deploy/pathplanner/autos/Score G L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
@@ -17,7 +17,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Start to Near G"
+                  "pathName": "Start to I"
                 }
               },
               {
@@ -55,12 +55,6 @@
             ]
           }
         },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Get to G"
-          }
-        },
         {
           "type": "named",
           "data": {
@@ -70,7 +64,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Back G"
+            "pathName": "Back I"
           }
         },
         {
@@ -80,7 +74,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "G to Source Y"
+                  "pathName": "I to Source Y"
                 }
               },
               {
diff --git a/src/main/deploy/pathplanner/autos/Score I L4.auto b/src/main/deploy/pathplanner/autos/Score I L4.auto
new file mode 100644
index 00000000..be8793b9
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score I L4.auto	
@@ -0,0 +1,76 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to I"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back I"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back I.path b/src/main/deploy/pathplanner/paths/Back I.path
new file mode 100644
index 00000000..fe706651
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Back I.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.255028409095894,
+        "y": 5.087
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.498074794941469,
+        "y": 5.145553004426791
+      },
+      "isLocked": false,
+      "linkedName": "I Reef"
+    },
+    {
+      "anchor": {
+        "x": 5.384659090909091,
+        "y": 5.27643465909091
+      },
+      "prevControl": {
+        "x": 5.124599400312309,
+        "y": 5.241484178437176
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near I"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/I to Source Y.path b/src/main/deploy/pathplanner/paths/I to Source Y.path
new file mode 100644
index 00000000..734728ff
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/I to Source Y.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.384659090909091,
+        "y": 5.27643465909091
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.651051136358651,
+        "y": 6.4431107954495594
+      },
+      "isLocked": false,
+      "linkedName": "Near I"
+    },
+    {
+      "anchor": {
+        "x": 1.1567045454545453,
+        "y": 7.091264204545454
+      },
+      "prevControl": {
+        "x": 0.8619328642546654,
+        "y": 7.1797656140891375
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Source Y"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.841814560191686
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to I.path b/src/main/deploy/pathplanner/paths/Start to I.path
new file mode 100644
index 00000000..9ed9f1c1
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Start to I.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.867585227272728,
+        "y": 6.183849431818182
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.813475322946259,
+        "y": 5.918559070511494
+      },
+      "isLocked": false,
+      "linkedName": "Right Start"
+    },
+    {
+      "anchor": {
+        "x": 5.255028409095894,
+        "y": 5.086974431823168
+      },
+      "prevControl": {
+        "x": 5.004079456292882,
+        "y": 4.938779683891955
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "I Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index be7285d9..143ffb14 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -40,7 +40,26 @@ public RobotConfigComp() {
                     Units.inchesToMeters(14.25),
                     Units.inchesToMeters(2.5),
                     Units.inchesToMeters(7.125)),
-                new Rotation3d(0.0, 0.0, 0.0))));
+                new Rotation3d(0.0, 5.0, 0.0))));
+    vision.addCamera(
+        new Camera(
+            "left_cam",
+            new Transform3d(
+                new Translation3d(
+                    Units.inchesToMeters(3.25),
+                    Units.inchesToMeters(13.5),
+                    Units.inchesToMeters(7.125)),
+                new Rotation3d(0.0, 5.0, 0.0))));
+
+    vision.addCamera(
+        new Camera(
+            "right_cam",
+            new Transform3d(
+                new Translation3d(
+                    Units.inchesToMeters(3.25),
+                    Units.inchesToMeters(-13.5),
+                    Units.inchesToMeters(7.0)),
+                new Rotation3d(0.0, 5.0, 0.0))));
     // Comp has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
     Drive.Constants.rotatePidKi = 0.0;
diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index e4ec9385..71f4d64d 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.interfaces.Vision;
 import java.util.ArrayList;
@@ -176,9 +177,14 @@ public void updatePoseEstimation() {
       Optional<EstimatedRobotPose> currentEstimatedRobotPose = camera.getEstimatedRobotPose();
       if (currentEstimatedRobotPose.isPresent()) {
         debugTargetDistance = camera.getDistanceToBestTarget();
-
+        double targetDistance;
+        if (RobotState.isAutonomous()) {
+          targetDistance = 5.0;
+        } else {
+          targetDistance = 0.8;
+        }
         // Add vision measurement to the consumer.
-        if (visionMeasurementConsumer != null && debugTargetDistance < 5) {
+        if (visionMeasurementConsumer != null && debugTargetDistance < targetDistance) {
           visionMeasurementConsumer.add(
               currentEstimatedRobotPose.get().estimatedPose.toPose2d(),
               currentEstimatedRobotPose.get().timestampSeconds,

From 737ded039c3544bd1839c54156720a2c668f71c7 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Mon, 3 Mar 2025 22:56:12 -0500
Subject: [PATCH 20/90] radial menu functionality for dynamically choosing
 target reef position

---
 .../controls/combination/DriverControls.java  | 103 +++++++++---------
 .../controls/drive/DriveControls.java         |  98 ++++++++++++-----
 2 files changed, 122 insertions(+), 79 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 802e528b..bebbe438 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -3,7 +3,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -21,6 +20,7 @@ public static class Constants {
 
     public static Command prepareScoreCommand = null;
   }
+
   public static void setupController(Elevator elevator, Arm arm, CommandXboxController controller) {
     Trigger ableToIntake =
         new Trigger(
@@ -43,67 +43,70 @@ public static void setupController(Elevator elevator, Arm arm, CommandXboxContro
     Command score = new ArmToPosition(arm, () -> 0);
     controller.rightTrigger().onTrue(score);
 
-    Constants.prepareScoreChooser.setDefaultOption(
-        "L2",
-        2);
+    Constants.prepareScoreChooser.setDefaultOption("L2", 2);
 
-    Constants.prepareScoreChooser.addOption(
-        "L3",
-        3);
+    Constants.prepareScoreChooser.addOption("L3", 3);
 
-    Constants.prepareScoreChooser.addOption(
-        "L4",
-        4);
-        
-    SelectCommand prepareScoreControllerCommand = new SelectCommand<>(
-          Map.ofEntries(
-            Map.entry(
-              2,
-              new SequentialCommandGroup(
-                new ElevatorToPosition(elevator, () -> 0.63), new ArmToPosition(arm, () -> 75)).withTimeout(5.0)
-              ),
-              Map.entry(
-              3,
-              new SequentialCommandGroup(
-                new ElevatorToPosition(elevator, () -> 1.0), new ArmToPosition(arm, () -> 75)).withTimeout(5.0)
-              ),
-              Map.entry(
-              4,
-              new SequentialCommandGroup(
-                new ElevatorToPosition(elevator, () -> 0.6),
-                new ParallelCommandGroup(
-                    new ArmToPosition(arm, () -> 75).withTimeout(1.0), new ElevatorToPosition(elevator, () -> 1.553))).withTimeout(5.0)
-              )),
-              () -> {
-                return Constants.prepareScoreChooser.getSelected();
-              });
+    Constants.prepareScoreChooser.addOption("L4", 4);
 
-              Constants.prepareScoreCommand = new SelectCommand<>(
-                Map.ofEntries(
-                  Map.entry(
+    SelectCommand prepareScoreControllerCommand =
+        new SelectCommand<>(
+            Map.ofEntries(
+                Map.entry(
                     2,
                     new SequentialCommandGroup(
-                      new ElevatorToPosition(elevator, () -> 0.63), new ArmToPosition(arm, () -> 75)).withTimeout(5.0)
-                    ),
-                    Map.entry(
+                            new ElevatorToPosition(elevator, () -> 0.63),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
                     3,
                     new SequentialCommandGroup(
-                      new ElevatorToPosition(elevator, () -> 1.0), new ArmToPosition(arm, () -> 75)).withTimeout(5.0)
-                    ),
-                    Map.entry(
+                            new ElevatorToPosition(elevator, () -> 1.0),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
                     4,
                     new SequentialCommandGroup(
-                      new ElevatorToPosition(elevator, () -> 0.6),
-                      new ParallelCommandGroup(
-                          new ArmToPosition(arm, () -> 75).withTimeout(1.0), new ElevatorToPosition(elevator, () -> 1.553))).withTimeout(5.0)
-                    )),
-                    () -> {
-                      return Constants.prepareScoreChooser.getSelected();
-                    });
+                            new ElevatorToPosition(elevator, () -> 0.6),
+                            new ParallelCommandGroup(
+                                new ArmToPosition(arm, () -> 75).withTimeout(1.0),
+                                new ElevatorToPosition(elevator, () -> 1.553)))
+                        .withTimeout(5.0))),
+            () -> {
+              return Constants.prepareScoreChooser.getSelected();
+            });
+
+    Constants.prepareScoreCommand =
+        new SelectCommand<>(
+            Map.ofEntries(
+                Map.entry(
+                    2,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 0.63),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
+                    3,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 1.0),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
+                    4,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 0.6),
+                            new ParallelCommandGroup(
+                                new ArmToPosition(arm, () -> 75).withTimeout(1.0),
+                                new ElevatorToPosition(elevator, () -> 1.553)))
+                        .withTimeout(5.0))),
+            () -> {
+              return Constants.prepareScoreChooser.getSelected();
+            });
     SmartDashboard.putData(
         "Driver " + "/Commands/Prepare To Score Command", prepareScoreControllerCommand);
 
-    SmartDashboard.putData("Driver " + "/Commands/Prepare To Score Chooser", Constants.prepareScoreChooser);
+    SmartDashboard.putData(
+        "Driver " + "/Commands/Prepare To Score Chooser", Constants.prepareScoreChooser);
 
     controller.y().onTrue(prepareScoreControllerCommand);
   }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 04c055e1..ea118913 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -9,15 +9,13 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.ProxyCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.drive.DriveCommand;
-import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.subsystems.controls.combination.DriverControls;
 import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Drive;
@@ -60,6 +58,12 @@ private TargetPoseOption(int idx) {
 
   protected static int myCoolPoseKeyIdx = 11;
 
+  protected static int coolestNumberEver = 0;
+
+  protected static final String[] orderedReefPositions = {
+    "E", "F", "G", "H", "I", "J", "K", "L", "A", "B", "C", "D"
+  };
+
   public static void setupController(
       Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
     SubsystemBase driveSubsystem = (SubsystemBase) drive;
@@ -218,13 +222,12 @@ public static void setupController(
                     TargetPoseOption.REEF_F.getIndex(),
                     AutoBuilder.pathfindToPose(poseReefF, constraints, 0.0)),
                 createDynamicPathScoringCommand(
-                  TargetPoseOption.REEF_H.getIndex(),
-                  poseReefHStart,
-                  poseReefHEnd,
-                  constraints,
-                  DriverControls.Constants.prepareScoreCommand,
-                  new ArmToPosition(arm, () -> 0)
-                ),
+                    TargetPoseOption.REEF_H.getIndex(),
+                    poseReefHStart,
+                    poseReefHEnd,
+                    constraints,
+                    DriverControls.Constants.prepareScoreCommand,
+                    new ArmToPosition(arm, () -> 0)),
                 // Map.entry(
                 //     TargetPoseOption.REEF_H.getIndex(),
                 //     AutoBuilder.pathfindToPose(poseReefHStart, constraints, 0.0)
@@ -263,28 +266,65 @@ public static void setupController(
 
     // dynamically go to destination
     controller.b().whileTrue(coolGoToPose);
+
+    /*  Angles -> Reef positions
+     * 0    30  : E
+     * 30   60  : F
+     * 60   90  : G
+     * 90   120 : H
+     * 120  150 : I
+     * 150  180 : J
+     * 180  210 : K
+     * 210  240 : L
+     * 240  270 : A
+     * 270  300 : B
+     * 300  330 : C
+     * 330  360 : D
+     */
+    controller
+        .x()
+        .whileTrue(
+            new RunCommand(
+                () -> {
+                  double myX = controller.getLeftX();
+                  double myY = -controller.getLeftY();
+
+                  // TODO maybe keep calculation in radians?
+                  double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
+                  if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
+
+                  // This number can be used to index into the ordered reef positions array!
+                  coolestNumberEver = (int) myNumber / 30;
+
+                  // TODO remove Smartdashboard number; only display reef position
+                  SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
+                  SmartDashboard.putString(
+                      "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
+                }));
   }
 
-  private static Entry<Integer, Command> createDynamicPathCommand(int index, Pose2d endingPose, PathConstraints constraints) {
-    Entry<Integer, Command> entry = 
-      Map.entry(
-        index,
-          AutoBuilder.pathfindToPose(endingPose, constraints, 0.0)
-      );
+  private static Entry<Integer, Command> createDynamicPathCommand(
+      int index, Pose2d endingPose, PathConstraints constraints) {
+    Entry<Integer, Command> entry =
+        Map.entry(index, AutoBuilder.pathfindToPose(endingPose, constraints, 0.0));
     return null;
   }
-  private static Entry<Integer, Command> createDynamicPathScoringCommand(int index, Pose2d initialPose, Pose2d scoringPose, PathConstraints constraints, Command prepareToScoreCommand, Command scoreCommand) {
-    Entry<Integer, Command> entry = 
-      Map.entry(
-        index,
-        new SequentialCommandGroup(
-          AutoBuilder.pathfindToPose(initialPose, constraints, 0.0),
-          prepareToScoreCommand,
-          AutoBuilder.pathfindToPose(scoringPose, constraints, 0.0),
-          scoreCommand
-        )
-      );
-      return entry;
-    
+
+  private static Entry<Integer, Command> createDynamicPathScoringCommand(
+      int index,
+      Pose2d initialPose,
+      Pose2d scoringPose,
+      PathConstraints constraints,
+      Command prepareToScoreCommand,
+      Command scoreCommand) {
+    Entry<Integer, Command> entry =
+        Map.entry(
+            index,
+            new SequentialCommandGroup(
+                AutoBuilder.pathfindToPose(initialPose, constraints, 0.0),
+                prepareToScoreCommand,
+                AutoBuilder.pathfindToPose(scoringPose, constraints, 0.0),
+                scoreCommand));
+    return entry;
   }
 }

From 26ac000f2339935e114df2a566a42efc53fbddfb Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Tue, 4 Mar 2025 19:56:10 -0500
Subject: [PATCH 21/90] Added Driver Camera

---
 .../game/reefscape2025/RobotConfigComp.java   |  46 ++++++-
 .../controls/combination/DriverControls.java  | 113 ++++++++++++++++++
 2 files changed, 157 insertions(+), 2 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 2e65db5b..b12a3a4b 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -1,8 +1,12 @@
 package frc.robot.config.game.reefscape2025;
 
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.util.Color;
@@ -19,11 +23,49 @@
 import frc.robot.subsystems.interfaces.Arm.ArmSettings;
 import frc.robot.subsystems.interfaces.Drive;
 import frc.robot.subsystems.interfaces.Elevator.ElevatorSettings;
+import frc.robot.subsystems.interfaces.Vision.Camera;
 
 public class RobotConfigComp extends RobotConfig {
   public RobotConfigComp() {
-    super(false, true, true, false, false, true, true);
-
+    super(false, false, false, false, false, true, true);
+    CameraServer.startAutomaticCapture();
+    vision.addCamera(
+        new Camera(
+            "front_cam", // front
+            new Transform3d(
+                new Translation3d(
+                    Units.inchesToMeters(13.592),
+                    Units.inchesToMeters(2.75),
+                    Units.inchesToMeters(7.201)),
+                new Rotation3d(0.0, Units.degreesToRadians(5.00), 0.0))));
+    vision.addCamera(
+        new Camera(
+            "left_cam", // left
+            new Transform3d(
+                new Translation3d(
+                    Units.inchesToMeters(3.250),
+                    Units.inchesToMeters(13.592),
+                    Units.inchesToMeters(7.201)),
+                new Rotation3d(0.0, Units.degreesToRadians(5.0), Units.degreesToRadians(90.0)))));
+
+    vision.addCamera(
+        new Camera(
+            "right_cam", // right
+            new Transform3d(
+                new Translation3d(
+                    Units.inchesToMeters(3.250),
+                    Units.inchesToMeters(-13.592),
+                    Units.inchesToMeters(7.201)),
+                new Rotation3d(0.0, Units.degreesToRadians(5.0), Units.degreesToRadians(270.0)))));
+    vision.addCamera(
+        new Camera(
+            "back_cam", // back
+            new Transform3d(
+                new Translation3d(
+                    Units.inchesToMeters(0.295),
+                    Units.inchesToMeters(-11.443),
+                    Units.inchesToMeters(39.663)),
+                new Rotation3d(Units.degreesToRadians(180.0), Units.degreesToRadians(33.0), Units.degreesToRadians(190)))));
     // Comp has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
     Drive.Constants.rotatePidKi = 0.0;
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
new file mode 100644
index 00000000..bebbe438
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -0,0 +1,113 @@
+package frc.robot.subsystems.controls.combination;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SelectCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.common.arm.ArmToPosition;
+import frc.robot.commands.common.elevator.ElevatorToPosition;
+import frc.robot.subsystems.interfaces.Arm;
+import frc.robot.subsystems.interfaces.Elevator;
+import java.util.Map;
+
+public class DriverControls {
+  public static class Constants {
+    public static SendableChooser<Integer> prepareScoreChooser = new SendableChooser<>();
+
+    public static Command prepareScoreCommand = null;
+  }
+
+  public static void setupController(Elevator elevator, Arm arm, CommandXboxController controller) {
+    Trigger ableToIntake =
+        new Trigger(
+            () -> {
+              if (elevator.getCurrentHeight() > 0.5) {
+                return true;
+              }
+              return false;
+            });
+
+    Command prepareIntakeCoralCommand =
+        new SequentialCommandGroup(
+            new ArmToPosition(arm, () -> -90).withTimeout(0),
+            new ElevatorToPosition(elevator, () -> 0.8));
+    controller.a().and(ableToIntake).onTrue(prepareIntakeCoralCommand);
+
+    Command intakeCoralCommand = new ElevatorToPosition(elevator, () -> 0.4);
+    controller.leftTrigger().onTrue(intakeCoralCommand);
+
+    Command score = new ArmToPosition(arm, () -> 0);
+    controller.rightTrigger().onTrue(score);
+
+    Constants.prepareScoreChooser.setDefaultOption("L2", 2);
+
+    Constants.prepareScoreChooser.addOption("L3", 3);
+
+    Constants.prepareScoreChooser.addOption("L4", 4);
+
+    SelectCommand prepareScoreControllerCommand =
+        new SelectCommand<>(
+            Map.ofEntries(
+                Map.entry(
+                    2,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 0.63),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
+                    3,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 1.0),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
+                    4,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 0.6),
+                            new ParallelCommandGroup(
+                                new ArmToPosition(arm, () -> 75).withTimeout(1.0),
+                                new ElevatorToPosition(elevator, () -> 1.553)))
+                        .withTimeout(5.0))),
+            () -> {
+              return Constants.prepareScoreChooser.getSelected();
+            });
+
+    Constants.prepareScoreCommand =
+        new SelectCommand<>(
+            Map.ofEntries(
+                Map.entry(
+                    2,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 0.63),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
+                    3,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 1.0),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
+                    4,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 0.6),
+                            new ParallelCommandGroup(
+                                new ArmToPosition(arm, () -> 75).withTimeout(1.0),
+                                new ElevatorToPosition(elevator, () -> 1.553)))
+                        .withTimeout(5.0))),
+            () -> {
+              return Constants.prepareScoreChooser.getSelected();
+            });
+    SmartDashboard.putData(
+        "Driver " + "/Commands/Prepare To Score Command", prepareScoreControllerCommand);
+
+    SmartDashboard.putData(
+        "Driver " + "/Commands/Prepare To Score Chooser", Constants.prepareScoreChooser);
+
+    controller.y().onTrue(prepareScoreControllerCommand);
+  }
+}

From 555b2cde40df82e6b4a0b7a2ab0c7339871f9c8c Mon Sep 17 00:00:00 2001
From: Carter Briscoe <tha.c.bug@gmail.com>
Date: Tue, 4 Mar 2025 20:28:58 -0500
Subject: [PATCH 22/90] made edit to navgrid so the robot will avoid a larger
 location around the reef

should not bump into reef now
---
 src/main/deploy/pathplanner/navgrid.json          | 2 +-
 src/main/deploy/yagsl/comp/modules/backright.json | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 23e0db94..adb2d744 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/yagsl/comp/modules/backright.json b/src/main/deploy/yagsl/comp/modules/backright.json
index 13316823..8e414824 100644
--- a/src/main/deploy/yagsl/comp/modules/backright.json
+++ b/src/main/deploy/yagsl/comp/modules/backright.json
@@ -3,7 +3,7 @@
     "front": -12.125,
     "left": -12.125
   },
-  "absoluteEncoderOffset": 333.676,
+  "absoluteEncoderOffset": 337.676,
   "drive": {
     "type": "sparkmax_neo",
     "id": 14,

From 1b98cdddd6beb44507c7a8e0e6c2c9d2841a393a Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Tue, 4 Mar 2025 21:04:41 -0500
Subject: [PATCH 23/90] Define command for on-the-fly scoring agnostic to
 chosen alliance

---
 .../deploy/yagsl/comp/modules/backright.json  |  2 +-
 .../controls/drive/DriveControls.java         | 33 +++++++--
 .../subsystems/controls/drive/TargetPose.java | 74 +++++++++++++++++++
 .../drive/DriveSwerveYAGSL.java               |  8 +-
 4 files changed, 102 insertions(+), 15 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java

diff --git a/src/main/deploy/yagsl/comp/modules/backright.json b/src/main/deploy/yagsl/comp/modules/backright.json
index 8e414824..13316823 100644
--- a/src/main/deploy/yagsl/comp/modules/backright.json
+++ b/src/main/deploy/yagsl/comp/modules/backright.json
@@ -3,7 +3,7 @@
     "front": -12.125,
     "left": -12.125
   },
-  "absoluteEncoderOffset": 337.676,
+  "absoluteEncoderOffset": 333.676,
   "drive": {
     "type": "sparkmax_neo",
     "id": 14,
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index ea118913..62601064 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -251,7 +251,6 @@ public static void setupController(
             () -> {
               return myCoolPoseKeyIdx;
             });
-    // ), () -> { return chooser.getSelected(); });
 
     // TODO: new dynamic path planning command creation
     // 1. SelectCommand ONE: go to initial location (based on user-chosen option X)
@@ -267,6 +266,13 @@ public static void setupController(
     // dynamically go to destination
     controller.b().whileTrue(coolGoToPose);
 
+    // TEMP FUNCTION TO TEST FIELD FLIPPING
+    // controller.b().whileTrue(new SequentialCommandGroup(
+    //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPrepPose(), constraints, 0.0),
+    //     // DriverControls.Constants.prepareScoreCommand,
+    //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPose(), constraints, 0.0),
+    //     new ArmToPosition(arm, () -> 0)));
+
     /*  Angles -> Reef positions
      * 0    30  : E
      * 30   60  : F
@@ -303,13 +309,6 @@ public static void setupController(
                 }));
   }
 
-  private static Entry<Integer, Command> createDynamicPathCommand(
-      int index, Pose2d endingPose, PathConstraints constraints) {
-    Entry<Integer, Command> entry =
-        Map.entry(index, AutoBuilder.pathfindToPose(endingPose, constraints, 0.0));
-    return null;
-  }
-
   private static Entry<Integer, Command> createDynamicPathScoringCommand(
       int index,
       Pose2d initialPose,
@@ -327,4 +326,22 @@ private static Entry<Integer, Command> createDynamicPathScoringCommand(
                 scoreCommand));
     return entry;
   }
+
+  private static Entry<Integer, Command> coolDynamicPathScoringCommand(
+      TargetPose target,
+      PathConstraints constraints,
+      Command prepareToScoreCommand,
+      Command scoreCommand) {
+    // FIXME initialize a fresh instance of the prepareToScoreCommand each time!!
+    // OTHERWISE CODE WILL CRASH
+    Entry<Integer, Command> entry =
+        Map.entry(
+            target.getIndex(),
+            new SequentialCommandGroup(
+                AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
+                prepareToScoreCommand,
+                AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
+                scoreCommand));
+    return entry;
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
new file mode 100644
index 00000000..e4f416d6
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -0,0 +1,74 @@
+package frc.robot.subsystems.controls.drive;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+public enum TargetPose {
+  // NOTE: WE ARE IDENTIFYING THESE POSITIONS ON THE BLUE SIDE OF THE FIELD!!!
+  // THEN, DEPENDING ON THE ALLIANCE GIVEN TO THE DRIVER STATION,
+  // PATHPLANNER WILL DECIDE WHETHER THE DESTINATION POSE IS
+  // ON THE RED OR BLUE SIDE OF THE FIELD
+  ORIGIN(0, "O", "Origin", new Pose2d(0.0, 0.0, new Rotation2d(0)), 0),
+  FEEDER_R(1, "FR", "Feeder Right", new Pose2d(1.05, 1, new Rotation2d(50)), 0),
+  FEEDER_L(2, "FL", "Feeder Left", new Pose2d(1.05, 7, Rotation2d.fromDegrees(-50)), 0),
+  REEF_A(3, "A", "Reef A", new Pose2d(3.25, 4.05, Rotation2d.fromDegrees(0)), -1),
+  REEF_G(9, "G", "Reef G", new Pose2d(5.5, 3.95, Rotation2d.fromDegrees(180)), -1),
+  PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0);
+
+  private int index;
+  private String shortName;
+  private String longName;
+
+  private Pose2d pose;
+  private Pose2d prepPose;
+  private Pose2d endPose;
+
+  // NOTE differential poses ~= 0.5m away from target
+
+  public int getIndex() {
+    return this.index;
+  }
+
+  public String getShortName() {
+    return this.shortName;
+  }
+
+  public String getLongName() {
+    return this.longName;
+  }
+
+  public Pose2d getPose() {
+    return this.pose;
+  }
+
+  public Pose2d getPrepPose() {
+    return this.prepPose;
+  }
+
+  public Pose2d getEndPose(boolean isRed) {
+    return this.endPose;
+  }
+
+  private TargetPose(int idx, String sName, String lName, Pose2d targetPose, int reefPosition) {
+    this.index = idx;
+    this.shortName = sName;
+    this.longName = lName;
+    this.pose = targetPose;
+
+    // TODO handle error case of accessing prep and end poses on non-reef position
+
+    // If this pose is not on the reef, don't set extra poses
+    if (reefPosition == 0) {
+      this.prepPose = null;
+      this.endPose = null;
+    } else {
+      double distance = (reefPosition < 0) ? -0.5 : 0.5;
+      this.prepPose =
+          targetPose.transformBy(new Transform2d(new Translation2d(0, distance), new Rotation2d()));
+      this.endPose =
+          targetPose.transformBy(new Transform2d(new Translation2d(-0.5, 0), new Rotation2d()));
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index 2df7da0a..a178ceda 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -11,13 +11,13 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import frc.robot.io.interfaces.DriveIO;
 import frc.robot.io.interfaces.DriveIOInputsAutoLogged;
 import frc.robot.subsystems.interfaces.Drive;
+import frc.robot.util.DevilBotState;
 import java.io.File;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -117,11 +117,7 @@ private boolean setupPathPlanner(
             // Boolean supplier that controls when the path will be mirrored for the red alliance
             // This will flip the path being followed to the red side of the field.
             // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
-            var alliance = DriverStation.getAlliance();
-            if (alliance.isPresent()) {
-              return alliance.get() == DriverStation.Alliance.Red;
-            }
-            return false;
+            return DevilBotState.isRedAlliance();
           },
           this // Reference to this subsystem to set requirements
           );

From 8f1b97fa88087ae7f93d33946a3ffc27cc4e238a Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Wed, 5 Mar 2025 15:49:57 -0500
Subject: [PATCH 24/90] Changed name of camera

---
 .../game/reefscape2025/RobotConfigComp.java   |   7 +-
 .../controls/combination/DriverControls.java  | 103 +++++++++---------
 2 files changed, 58 insertions(+), 52 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index b12a3a4b..913e922c 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -59,13 +59,16 @@ public RobotConfigComp() {
                 new Rotation3d(0.0, Units.degreesToRadians(5.0), Units.degreesToRadians(270.0)))));
     vision.addCamera(
         new Camera(
-            "back_cam", // back
+            "rear_cam", // rear
             new Transform3d(
                 new Translation3d(
                     Units.inchesToMeters(0.295),
                     Units.inchesToMeters(-11.443),
                     Units.inchesToMeters(39.663)),
-                new Rotation3d(Units.degreesToRadians(180.0), Units.degreesToRadians(33.0), Units.degreesToRadians(190)))));
+                new Rotation3d(
+                    Units.degreesToRadians(180.0),
+                    Units.degreesToRadians(33.0),
+                    Units.degreesToRadians(190.0)))));
     // Comp has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
     Drive.Constants.rotatePidKi = 0.0;
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 802e528b..bebbe438 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -3,7 +3,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -21,6 +20,7 @@ public static class Constants {
 
     public static Command prepareScoreCommand = null;
   }
+
   public static void setupController(Elevator elevator, Arm arm, CommandXboxController controller) {
     Trigger ableToIntake =
         new Trigger(
@@ -43,67 +43,70 @@ public static void setupController(Elevator elevator, Arm arm, CommandXboxContro
     Command score = new ArmToPosition(arm, () -> 0);
     controller.rightTrigger().onTrue(score);
 
-    Constants.prepareScoreChooser.setDefaultOption(
-        "L2",
-        2);
+    Constants.prepareScoreChooser.setDefaultOption("L2", 2);
 
-    Constants.prepareScoreChooser.addOption(
-        "L3",
-        3);
+    Constants.prepareScoreChooser.addOption("L3", 3);
 
-    Constants.prepareScoreChooser.addOption(
-        "L4",
-        4);
-        
-    SelectCommand prepareScoreControllerCommand = new SelectCommand<>(
-          Map.ofEntries(
-            Map.entry(
-              2,
-              new SequentialCommandGroup(
-                new ElevatorToPosition(elevator, () -> 0.63), new ArmToPosition(arm, () -> 75)).withTimeout(5.0)
-              ),
-              Map.entry(
-              3,
-              new SequentialCommandGroup(
-                new ElevatorToPosition(elevator, () -> 1.0), new ArmToPosition(arm, () -> 75)).withTimeout(5.0)
-              ),
-              Map.entry(
-              4,
-              new SequentialCommandGroup(
-                new ElevatorToPosition(elevator, () -> 0.6),
-                new ParallelCommandGroup(
-                    new ArmToPosition(arm, () -> 75).withTimeout(1.0), new ElevatorToPosition(elevator, () -> 1.553))).withTimeout(5.0)
-              )),
-              () -> {
-                return Constants.prepareScoreChooser.getSelected();
-              });
+    Constants.prepareScoreChooser.addOption("L4", 4);
 
-              Constants.prepareScoreCommand = new SelectCommand<>(
-                Map.ofEntries(
-                  Map.entry(
+    SelectCommand prepareScoreControllerCommand =
+        new SelectCommand<>(
+            Map.ofEntries(
+                Map.entry(
                     2,
                     new SequentialCommandGroup(
-                      new ElevatorToPosition(elevator, () -> 0.63), new ArmToPosition(arm, () -> 75)).withTimeout(5.0)
-                    ),
-                    Map.entry(
+                            new ElevatorToPosition(elevator, () -> 0.63),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
                     3,
                     new SequentialCommandGroup(
-                      new ElevatorToPosition(elevator, () -> 1.0), new ArmToPosition(arm, () -> 75)).withTimeout(5.0)
-                    ),
-                    Map.entry(
+                            new ElevatorToPosition(elevator, () -> 1.0),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
                     4,
                     new SequentialCommandGroup(
-                      new ElevatorToPosition(elevator, () -> 0.6),
-                      new ParallelCommandGroup(
-                          new ArmToPosition(arm, () -> 75).withTimeout(1.0), new ElevatorToPosition(elevator, () -> 1.553))).withTimeout(5.0)
-                    )),
-                    () -> {
-                      return Constants.prepareScoreChooser.getSelected();
-                    });
+                            new ElevatorToPosition(elevator, () -> 0.6),
+                            new ParallelCommandGroup(
+                                new ArmToPosition(arm, () -> 75).withTimeout(1.0),
+                                new ElevatorToPosition(elevator, () -> 1.553)))
+                        .withTimeout(5.0))),
+            () -> {
+              return Constants.prepareScoreChooser.getSelected();
+            });
+
+    Constants.prepareScoreCommand =
+        new SelectCommand<>(
+            Map.ofEntries(
+                Map.entry(
+                    2,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 0.63),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
+                    3,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 1.0),
+                            new ArmToPosition(arm, () -> 75))
+                        .withTimeout(5.0)),
+                Map.entry(
+                    4,
+                    new SequentialCommandGroup(
+                            new ElevatorToPosition(elevator, () -> 0.6),
+                            new ParallelCommandGroup(
+                                new ArmToPosition(arm, () -> 75).withTimeout(1.0),
+                                new ElevatorToPosition(elevator, () -> 1.553)))
+                        .withTimeout(5.0))),
+            () -> {
+              return Constants.prepareScoreChooser.getSelected();
+            });
     SmartDashboard.putData(
         "Driver " + "/Commands/Prepare To Score Command", prepareScoreControllerCommand);
 
-    SmartDashboard.putData("Driver " + "/Commands/Prepare To Score Chooser", Constants.prepareScoreChooser);
+    SmartDashboard.putData(
+        "Driver " + "/Commands/Prepare To Score Chooser", Constants.prepareScoreChooser);
 
     controller.y().onTrue(prepareScoreControllerCommand);
   }

From 876a5e441d08ea77e234baba8c34e6dce11ffb2d Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Wed, 5 Mar 2025 19:02:58 -0500
Subject: [PATCH 25/90] reef A

---
 .../game/reefscape2025/RobotConfig.java       |  1 +
 .../controls/combination/DriverControls.java  | 32 +++++++++-
 .../controls/drive/DriveControls.java         | 64 ++++++++++---------
 .../subsystems/controls/drive/TargetPose.java |  7 +-
 4 files changed, 70 insertions(+), 34 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index cf9cd459..c9a8605a 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -215,6 +215,7 @@ public void configureBindings() {
     DriverAssistControls.setupController(elevator, coralArm, assistController);
     DriverControls.setupController(elevator, coralArm, mainController);
     DriveControls.setupController(drive, elevator, coralArm, mainController);
+    DriveControls.setupAssistantController(drive, assistController);
     CoralArmControls.setupController(coralArm, mainController);
     ElevatorControls.setupController(elevator, mainController);
     CoralArmControls.setupController(coralArm, assistController);
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index d939fa14..16301236 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -18,7 +19,7 @@ public class DriverControls {
   public static class Constants {
     public static SendableChooser<Integer> prepareScoreChooser = new SendableChooser<>();
     public static Integer prepareScoreSelctedIndex = 2;
-    public static Command prepareScoreCommand = null;
+    public static Command prepareScoreCommand = new InstantCommand();
   }
 
   public static void setupController(Elevator elevator, Arm arm, CommandXboxController controller) {
@@ -115,4 +116,33 @@ public static void setupController(Elevator elevator, Arm arm, CommandXboxContro
 
     controller.y().onTrue(prepareScoreControllerCommand);
   }
+  
+
+  public static Command getPrepareToScoreCommand(Elevator elevator, Arm arm) {
+    return new SelectCommand<>(
+        Map.ofEntries(
+            Map.entry(
+                2,
+                new SequentialCommandGroup(
+                        new ElevatorToPosition(elevator, () -> 0.63),
+                        new ArmToPosition(arm, () -> 75))
+                    .withTimeout(5.0)),
+            Map.entry(
+                3,
+                new SequentialCommandGroup(
+                        new ElevatorToPosition(elevator, () -> 1.0),
+                        new ArmToPosition(arm, () -> 75))
+                    .withTimeout(5.0)),
+            Map.entry(
+                4,
+                new SequentialCommandGroup(
+                        new ElevatorToPosition(elevator, () -> 0.6),
+                        new ParallelCommandGroup(
+                            new ArmToPosition(arm, () -> 75).withTimeout(1.0),
+                            new ElevatorToPosition(elevator, () -> 1.553)))
+                    .withTimeout(5.0))),
+        () -> {
+          return Constants.prepareScoreSelctedIndex;
+        });
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 62601064..9192ce0b 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -56,7 +56,7 @@ private TargetPoseOption(int idx) {
     }
   }
 
-  protected static int myCoolPoseKeyIdx = 11;
+  protected static int myCoolPoseKeyIdx = 3;
 
   protected static int coolestNumberEver = 0;
 
@@ -183,17 +183,16 @@ public static void setupController(
                     TargetPoseOption.FEEDER_2.getIndex(),
                     AutoBuilder.pathfindToPose(poseFeeder2, constraints, 0.0)),
                 Map.entry(
-                    TargetPoseOption.PROCESSOR.getIndex(),
+                    4, // processor
                     AutoBuilder.pathfindToPose(poseProcessor, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.REEF_A.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0)),
-                // AutoBuilder.pathfindToPose(poseReefA, constraints, 0.0)
+                coolDynamicPathScoringCommand(TargetPose.REEF_A, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
+                    // AutoBuilder.pathfindToPose(poseReefA, constraints, 0.0)
                 //     .andThen(AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0))),
-                Map.entry(
-                    TargetPoseOption.REEF_G.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
-                        .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
+                // Map.entry(
+                //     TargetPoseOption.REEF_G.getIndex(),
+                //     AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
+                //         .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
+                coolDynamicPathScoringCommand(TargetPose.REEF_G, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
                 // AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)),
                 Map.entry(
                     TargetPoseOption.REEF_C.getIndex(),
@@ -219,7 +218,7 @@ public static void setupController(
                     TargetPoseOption.REEF_E.getIndex(),
                     AutoBuilder.pathfindToPose(poseReefE, constraints, 0.0)),
                 Map.entry(
-                    TargetPoseOption.REEF_F.getIndex(),
+                    10, // need reee
                     AutoBuilder.pathfindToPose(poseReefF, constraints, 0.0)),
                 createDynamicPathScoringCommand(
                     TargetPoseOption.REEF_H.getIndex(),
@@ -287,27 +286,31 @@ public static void setupController(
      * 300  330 : C
      * 330  360 : D
      */
-    controller
-        .x()
-        .whileTrue(
-            new RunCommand(
-                () -> {
-                  double myX = controller.getLeftX();
-                  double myY = -controller.getLeftY();
+  }
 
-                  // TODO maybe keep calculation in radians?
-                  double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
-                  if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
+  public static void setupAssistantController(
+      Drive drive, CommandXboxController controller) {
+    controller
+            .x()
+            .whileTrue(
+                new RunCommand(
+                    () -> {
+                    double myX = controller.getLeftX();
+                    double myY = -controller.getLeftY();
 
-                  // This number can be used to index into the ordered reef positions array!
-                  coolestNumberEver = (int) myNumber / 30;
+                    // TODO maybe keep calculation in radians?
+                    double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
+                    if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
 
-                  // TODO remove Smartdashboard number; only display reef position
-                  SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
-                  SmartDashboard.putString(
-                      "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
-                }));
-  }
+                    // This number can be used to index into the ordered reef positions array!
+                    coolestNumberEver = (int) myNumber / 30;
+                    
+                    // TODO remove Smartdashboard number; only display reef position
+                    SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
+                    SmartDashboard.putString(
+                        "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
+                    }));
+            }
 
   private static Entry<Integer, Command> createDynamicPathScoringCommand(
       int index,
@@ -341,7 +344,8 @@ private static Entry<Integer, Command> coolDynamicPathScoringCommand(
                 AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
                 prepareToScoreCommand,
                 AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
-                scoreCommand));
+                scoreCommand
+                ));
     return entry;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index e4f416d6..7099e99b 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -13,7 +13,8 @@ public enum TargetPose {
   ORIGIN(0, "O", "Origin", new Pose2d(0.0, 0.0, new Rotation2d(0)), 0),
   FEEDER_R(1, "FR", "Feeder Right", new Pose2d(1.05, 1, new Rotation2d(50)), 0),
   FEEDER_L(2, "FL", "Feeder Left", new Pose2d(1.05, 7, Rotation2d.fromDegrees(-50)), 0),
-  REEF_A(3, "A", "Reef A", new Pose2d(3.25, 4.05, Rotation2d.fromDegrees(0)), -1),
+  REEF_A(3, "A", "Reef A", new Pose2d(3.15, 4.28, Rotation2d.fromDegrees(0)), 
+  -1),
   REEF_G(9, "G", "Reef G", new Pose2d(5.5, 3.95, Rotation2d.fromDegrees(180)), -1),
   PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0);
 
@@ -64,11 +65,11 @@ private TargetPose(int idx, String sName, String lName, Pose2d targetPose, int r
       this.prepPose = null;
       this.endPose = null;
     } else {
-      double distance = (reefPosition < 0) ? -0.5 : 0.5;
+      double distance = (reefPosition < 0) ? -0.7 : 0.7;
       this.prepPose =
           targetPose.transformBy(new Transform2d(new Translation2d(0, distance), new Rotation2d()));
       this.endPose =
-          targetPose.transformBy(new Transform2d(new Translation2d(-0.5, 0), new Rotation2d()));
+          targetPose.transformBy(new Transform2d(new Translation2d(0, 0), new Rotation2d()));
     }
   }
 }

From 698563b8221d9f75fe66b7ae81a272cce47ae6b5 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Wed, 5 Mar 2025 20:44:26 -0500
Subject: [PATCH 26/90] Only go to A position, update elastic layout

---
 src/main/deploy/elastic-layout.json           | 209 ++++++++----------
 .../controls/combination/DriverControls.java  |   1 -
 .../controls/drive/DriveControls.java         |  81 +++----
 3 files changed, 141 insertions(+), 150 deletions(-)

diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json
index d22ee3af..2bc267a6 100644
--- a/src/main/deploy/elastic-layout.json
+++ b/src/main/deploy/elastic-layout.json
@@ -1831,118 +1831,7 @@
     {
       "name": "Drive",
       "grid_layout": {
-        "layouts": [
-          {
-            "title": "rotationPID",
-            "x": 896.0,
-            "y": 256.0,
-            "width": 256.0,
-            "height": 256.0,
-            "type": "List Layout",
-            "properties": {
-              "label_position": "TOP"
-            },
-            "children": [
-              {
-                "title": "P",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/rotationPID/P",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "I",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/rotationPID/I",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "D",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/rotationPID/D",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              }
-            ]
-          },
-          {
-            "title": "translationPID",
-            "x": 896.0,
-            "y": 0.0,
-            "width": 256.0,
-            "height": 256.0,
-            "type": "List Layout",
-            "properties": {
-              "label_position": "TOP"
-            },
-            "children": [
-              {
-                "title": "P",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/translationPID/P",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "I",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/translationPID/I",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "D",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/translationPID/D",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              }
-            ]
-          }
-        ],
+        "layouts": [],
         "containers": [
           {
             "title": "Field",
@@ -1963,6 +1852,102 @@
               "robot_color": 4294198070,
               "trajectory_color": 4294967295
             }
+          },
+          {
+            "title": "PoseX",
+            "x": 0.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/PoseX",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "PoseY",
+            "x": 128.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/PoseY",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "PoseRotInDegrees",
+            "x": 256.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/PoseRotInDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Arm Position (Degrees)",
+            "x": 896.0,
+            "y": 0.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Arm[Coral]/PositionDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Elevator Pos (Meters)",
+            "x": 896.0,
+            "y": 128.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Elevator[Coral]/PositionMeters",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Auto Calibrate Coral Arm",
+            "x": 1088.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Arm[Coral]/Commands/Auto Calibrate Coral Arm",
+              "period": 0.06,
+              "show_type": true
+            }
+          },
+          {
+            "title": "Auto Calibrate Elevator",
+            "x": 1088.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Elevator[Coral]/Commands/Auto Calibrate Elevator",
+              "period": 0.06,
+              "show_type": true
+            }
           }
         ]
       }
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 16301236..b78871a6 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -116,7 +116,6 @@ public static void setupController(Elevator elevator, Arm arm, CommandXboxContro
 
     controller.y().onTrue(prepareScoreControllerCommand);
   }
-  
 
   public static Command getPrepareToScoreCommand(Elevator elevator, Arm arm) {
     return new SelectCommand<>(
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 9192ce0b..aad6c970 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -56,7 +56,7 @@ private TargetPoseOption(int idx) {
     }
   }
 
-  protected static int myCoolPoseKeyIdx = 3;
+  protected static int myCoolPoseKeyIdx = TargetPose.REEF_A.getIndex();
 
   protected static int coolestNumberEver = 0;
 
@@ -157,17 +157,18 @@ public static void setupController(
           myCoolPoseKeyIdx = chosenPose.getIndex();
           SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
         });
-    controller
-        .x()
-        .onTrue(
-            new InstantCommand(
-                () -> {
-                  if (myCoolPoseKeyIdx == 11) myCoolPoseKeyIdx = 2;
-                  else myCoolPoseKeyIdx = 11;
-                  //   if (++myCoolPoseKeyIdx == TargetPoseOption.values().length) myCoolPoseKeyIdx
-                  // = 1;
-                  SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
-                }));
+    // controller
+    //     .x()
+    //     .onTrue(
+    //         new InstantCommand(
+    //             () -> {
+    //               if (myCoolPoseKeyIdx == 11) myCoolPoseKeyIdx = 2;
+    //               else myCoolPoseKeyIdx = 11;
+    //               //   if (++myCoolPoseKeyIdx == TargetPoseOption.values().length)
+    // myCoolPoseKeyIdx
+    //               // = 1;
+    //               SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
+    //             }));
 
     // Define command to go to specific pose
     Command coolGoToPose =
@@ -185,14 +186,22 @@ public static void setupController(
                 Map.entry(
                     4, // processor
                     AutoBuilder.pathfindToPose(poseProcessor, constraints, 0.0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_A, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                    // AutoBuilder.pathfindToPose(poseReefA, constraints, 0.0)
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_A,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                // AutoBuilder.pathfindToPose(poseReefA, constraints, 0.0)
                 //     .andThen(AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0))),
                 // Map.entry(
                 //     TargetPoseOption.REEF_G.getIndex(),
                 //     AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
                 //         .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
-                coolDynamicPathScoringCommand(TargetPose.REEF_G, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_G,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
                 // AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)),
                 Map.entry(
                     TargetPoseOption.REEF_C.getIndex(),
@@ -288,29 +297,28 @@ public static void setupController(
      */
   }
 
-  public static void setupAssistantController(
-      Drive drive, CommandXboxController controller) {
+  public static void setupAssistantController(Drive drive, CommandXboxController controller) {
     controller
-            .x()
-            .whileTrue(
-                new RunCommand(
-                    () -> {
-                    double myX = controller.getLeftX();
-                    double myY = -controller.getLeftY();
+        .x()
+        .whileTrue(
+            new RunCommand(
+                () -> {
+                  double myX = controller.getLeftX();
+                  double myY = -controller.getLeftY();
 
-                    // TODO maybe keep calculation in radians?
-                    double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
-                    if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
+                  // TODO maybe keep calculation in radians?
+                  double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
+                  if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
 
-                    // This number can be used to index into the ordered reef positions array!
-                    coolestNumberEver = (int) myNumber / 30;
-                    
-                    // TODO remove Smartdashboard number; only display reef position
-                    SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
-                    SmartDashboard.putString(
-                        "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
-                    }));
-            }
+                  // This number can be used to index into the ordered reef positions array!
+                  coolestNumberEver = (int) myNumber / 30;
+
+                  // TODO remove Smartdashboard number; only display reef position
+                  SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
+                  SmartDashboard.putString(
+                      "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
+                }));
+  }
 
   private static Entry<Integer, Command> createDynamicPathScoringCommand(
       int index,
@@ -344,8 +352,7 @@ private static Entry<Integer, Command> coolDynamicPathScoringCommand(
                 AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
                 prepareToScoreCommand,
                 AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
-                scoreCommand
-                ));
+                scoreCommand));
     return entry;
   }
 }

From 2d73146abea8b0f6047fbc815d02612368ec55db Mon Sep 17 00:00:00 2001
From: Carter Briscoe <tha.c.bug@gmail.com>
Date: Wed, 5 Mar 2025 21:06:32 -0500
Subject: [PATCH 27/90] reef positions for other side

---
 .../controls/drive/DriveControls.java         |  4 ++--
 .../subsystems/controls/drive/TargetPose.java | 23 ++++++++++++++++++-
 2 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 9192ce0b..44adebca 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -164,8 +164,8 @@ public static void setupController(
                 () -> {
                   if (myCoolPoseKeyIdx == 11) myCoolPoseKeyIdx = 2;
                   else myCoolPoseKeyIdx = 11;
-                  //   if (++myCoolPoseKeyIdx == TargetPoseOption.values().length) myCoolPoseKeyIdx
-                  // = 1;
+                    if (++myCoolPoseKeyIdx == TargetPoseOption.values().length) myCoolPoseKeyIdx
+                  = 1;
                   SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
                 }));
 
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 7099e99b..b748e1c4 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -16,7 +16,28 @@ public enum TargetPose {
   REEF_A(3, "A", "Reef A", new Pose2d(3.15, 4.28, Rotation2d.fromDegrees(0)), 
   -1),
   REEF_G(9, "G", "Reef G", new Pose2d(5.5, 3.95, Rotation2d.fromDegrees(180)), -1),
-  PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0);
+  PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0),
+  REEF_B(4, "B", "Reef B", new Pose2d(3.15, 3.70, Rotation2d.fromDegrees(0)), 
+  0),
+  REEF_C(5, "C", "Reef C", new Pose2d(3.60, 2.93, Rotation2d.fromDegrees(60)), 
+  -1),
+  REEF_D(6, "D", "Reef D", new Pose2d(3.96, 2.74, Rotation2d.fromDegrees(60)), 
+  0),
+  REEF_E(7, "E", "Reef E", new Pose2d(5.10, 2.75, Rotation2d.fromDegrees(124)), 
+  -1),
+  REEF_F(8, "F", "Reef F", new Pose2d(5.36, 2.93, Rotation2d.fromDegrees(124)), 
+  0),
+  REEF_H(10, "H", "Reef H", new Pose2d(5.80, 4.20, Rotation2d.fromDegrees(180)), 
+  0),
+  REEF_I(11, "I", "Reef I", new Pose2d(5.30, 5.15, Rotation2d.fromDegrees(-124)), 
+  -1),
+  REEF_J(12, "J", "Reef J", new Pose2d(5.00, 5.30, Rotation2d.fromDegrees(-124)), 
+  0),
+  REEF_K(13, "K", "Reef K", new Pose2d(4.00, 5.33, Rotation2d.fromDegrees(-60)), 
+-1),
+  REEF_L(14, "L", "Reef L", new Pose2d(3.70, 5.20, Rotation2d.fromDegrees(-60)), 
+  0);
+  
 
   private int index;
   private String shortName;

From 0382942a8496eb2ee0f3000b7381b1ae5ded5002 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Thu, 6 Mar 2025 15:50:32 -0500
Subject: [PATCH 28/90] Fixed I Reef Auto and Works in field. Needs to be
 tested with back camera

---
 simulation.properties                         |  2 +-
 .../autos/Score I L4 + Y + L L4 .auto         |  8 ++-
 .../deploy/pathplanner/autos/Score I L4.auto  |  8 ++-
 src/main/deploy/pathplanner/paths/Back I.path | 16 +++---
 .../pathplanner/paths/G to Source Y.path      |  8 +--
 .../deploy/pathplanner/paths/Get to I.path    | 54 +++++++++++++++++++
 .../pathplanner/paths/I to Source Y.path      | 16 +++---
 .../pathplanner/paths/Source Y to L.path      |  8 +--
 .../{Start to I.path => Start to Near I.path} | 14 ++---
 .../game/reefscape2025/RobotConfigComp.java   |  4 +-
 .../vision/VisionSubsystem.java               | 10 +---
 11 files changed, 104 insertions(+), 44 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/Get to I.path
 rename src/main/deploy/pathplanner/paths/{Start to I.path => Start to Near I.path} (80%)

diff --git a/simulation.properties b/simulation.properties
index b731d52c..2d763eed 100644
--- a/simulation.properties
+++ b/simulation.properties
@@ -1 +1 @@
-robot.name=NEMO
\ No newline at end of file
+robot.name=STUB
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto
index bcb732f3..7bd9e6a7 100644
--- a/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
@@ -17,7 +17,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Start to I"
+                  "pathName": "Start to Near I"
                 }
               },
               {
@@ -55,6 +55,12 @@
             ]
           }
         },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to I"
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/Score I L4.auto b/src/main/deploy/pathplanner/autos/Score I L4.auto
index be8793b9..b676e942 100644
--- a/src/main/deploy/pathplanner/autos/Score I L4.auto	
+++ b/src/main/deploy/pathplanner/autos/Score I L4.auto	
@@ -17,7 +17,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Start to I"
+                  "pathName": "Start to Near I"
                 }
               },
               {
@@ -55,6 +55,12 @@
             ]
           }
         },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to I"
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/deploy/pathplanner/paths/Back I.path b/src/main/deploy/pathplanner/paths/Back I.path
index fe706651..9f9ceae7 100644
--- a/src/main/deploy/pathplanner/paths/Back I.path	
+++ b/src/main/deploy/pathplanner/paths/Back I.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.255028409095894,
-        "y": 5.087
+        "x": 5.354744318181819,
+        "y": 5.017173295454545
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.498074794941469,
-        "y": 5.145553004426791
+        "x": 5.596402748185905,
+        "y": 5.478657531140626
       },
       "isLocked": false,
       "linkedName": "I Reef"
     },
     {
       "anchor": {
-        "x": 5.384659090909091,
-        "y": 5.27643465909091
+        "x": 5.544204545454546,
+        "y": 5.376150568181817
       },
       "prevControl": {
-        "x": 5.124599400312309,
-        "y": 5.241484178437176
+        "x": 5.318787094807959,
+        "y": 4.950391851822856
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/G to Source Y.path b/src/main/deploy/pathplanner/paths/G to Source Y.path
index 436c7bc7..51bb6504 100644
--- a/src/main/deploy/pathplanner/paths/G to Source Y.path	
+++ b/src/main/deploy/pathplanner/paths/G to Source Y.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.1567045454545453,
-        "y": 7.091264204545454
+        "x": 1.0370454545454546,
+        "y": 7.171036931818182
       },
       "prevControl": {
-        "x": 4.0185511363636355,
-        "y": 6.562769886363637
+        "x": 3.898892045454545,
+        "y": 6.642542613636365
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Get to I.path b/src/main/deploy/pathplanner/paths/Get to I.path
new file mode 100644
index 00000000..c06f51ac
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Get to I.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.544204545454546,
+        "y": 5.376150568181817
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.39185046049515,
+        "y": 5.1779379732334005
+      },
+      "isLocked": false,
+      "linkedName": "Near I"
+    },
+    {
+      "anchor": {
+        "x": 5.354744318181819,
+        "y": 5.017173295454545
+      },
+      "prevControl": {
+        "x": 5.517467341821426,
+        "y": 5.213705553302059
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "I Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/I to Source Y.path b/src/main/deploy/pathplanner/paths/I to Source Y.path
index 734728ff..f7a00bb9 100644
--- a/src/main/deploy/pathplanner/paths/I to Source Y.path	
+++ b/src/main/deploy/pathplanner/paths/I to Source Y.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.384659090909091,
-        "y": 5.27643465909091
+        "x": 5.544204545454546,
+        "y": 5.376150568181817
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.651051136358651,
-        "y": 6.4431107954495594
+        "x": 6.810596590904106,
+        "y": 6.542826704540467
       },
       "isLocked": false,
       "linkedName": "Near I"
     },
     {
       "anchor": {
-        "x": 1.1567045454545453,
-        "y": 7.091264204545454
+        "x": 1.0370454545454546,
+        "y": 7.171036931818182
       },
       "prevControl": {
-        "x": 0.8619328642546654,
-        "y": 7.1797656140891375
+        "x": 0.7422737733455748,
+        "y": 7.259538341361865
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Source Y to L.path b/src/main/deploy/pathplanner/paths/Source Y to L.path
index 28e7c231..50a68e7c 100644
--- a/src/main/deploy/pathplanner/paths/Source Y to L.path	
+++ b/src/main/deploy/pathplanner/paths/Source Y to L.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.1567045454545453,
-        "y": 7.091264204545454
+        "x": 1.0370454545454546,
+        "y": 7.171036931818182
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.3006164708815255,
-        "y": 6.221119673625105
+        "x": 2.180957379972435,
+        "y": 6.300892400897832
       },
       "isLocked": false,
       "linkedName": "Source Y"
diff --git a/src/main/deploy/pathplanner/paths/Start to I.path b/src/main/deploy/pathplanner/paths/Start to Near I.path
similarity index 80%
rename from src/main/deploy/pathplanner/paths/Start to I.path
rename to src/main/deploy/pathplanner/paths/Start to Near I.path
index 9ed9f1c1..0d8f83eb 100644
--- a/src/main/deploy/pathplanner/paths/Start to I.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Near I.path	
@@ -8,24 +8,24 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.813475322946259,
-        "y": 5.918559070511494
+        "x": 6.891051929760262,
+        "y": 6.002461229032797
       },
       "isLocked": false,
       "linkedName": "Right Start"
     },
     {
       "anchor": {
-        "x": 5.255028409095894,
-        "y": 5.086974431823168
+        "x": 5.544204545454546,
+        "y": 5.376150568181817
       },
       "prevControl": {
-        "x": 5.004079456292882,
-        "y": 4.938779683891955
+        "x": 5.896167484058749,
+        "y": 5.4938919104517545
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "I Reef"
+      "linkedName": "Near I"
     }
   ],
   "rotationTargets": [],
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 627bc862..bca47ee0 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -71,8 +71,8 @@ public RobotConfigComp() {
                     Units.inchesToMeters(39.663)),
                 new Rotation3d(
                     Units.degreesToRadians(180.0),
-                    Units.degreesToRadians(33.0),
-                    Units.degreesToRadians(190.0)))));
+                    Units.degreesToRadians(-33.0),
+                    Units.degreesToRadians(170.0)))));
     // Comp has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
     Drive.Constants.rotatePidKi = 0.0;
diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index 71f4d64d..41e9ddc8 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -6,7 +6,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.interfaces.Vision;
 import java.util.ArrayList;
@@ -177,14 +176,9 @@ public void updatePoseEstimation() {
       Optional<EstimatedRobotPose> currentEstimatedRobotPose = camera.getEstimatedRobotPose();
       if (currentEstimatedRobotPose.isPresent()) {
         debugTargetDistance = camera.getDistanceToBestTarget();
-        double targetDistance;
-        if (RobotState.isAutonomous()) {
-          targetDistance = 5.0;
-        } else {
-          targetDistance = 0.8;
-        }
+
         // Add vision measurement to the consumer.
-        if (visionMeasurementConsumer != null && debugTargetDistance < targetDistance) {
+        if (visionMeasurementConsumer != null && debugTargetDistance < 1.5) {
           visionMeasurementConsumer.add(
               currentEstimatedRobotPose.get().estimatedPose.toPose2d(),
               currentEstimatedRobotPose.get().timestampSeconds,

From 10438472a07afc08e9ab3a6b1c3bb48d955b4022 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Thu, 6 Mar 2025 17:40:18 -0500
Subject: [PATCH 29/90] cleanup vision subsystem; improved logging

---
 .../game/reefscape2025/RobotConfig.java       |   2 +-
 .../game/reefscape2025/RobotConfigComp.java   |   4 -
 .../game/reefscape2025/RobotConfigNemo.java   |   4 -
 .../java/frc/robot/io/interfaces/DriveIO.java |  15 --
 .../subsystems/controls/drive/TargetPose.java |  34 ++---
 .../controls/drive/TargetPoseOLD.java         | 127 ++++++++++++++++
 .../vision/VisionSubsystem.java               | 135 +++---------------
 .../robot/subsystems/interfaces/Vision.java   |  33 -----
 8 files changed, 156 insertions(+), 198 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/controls/drive/TargetPoseOLD.java

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index 5f2b0e6f..1b634e64 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -26,8 +26,8 @@
 import frc.robot.subsystems.controls.arm.ClimberArmControls;
 import frc.robot.subsystems.controls.arm.CoralArmControls;
 import frc.robot.subsystems.controls.combination.DriverAssistControls;
-import frc.robot.subsystems.controls.combination.PitControls;
 import frc.robot.subsystems.controls.combination.DriverControls;
+import frc.robot.subsystems.controls.combination.PitControls;
 import frc.robot.subsystems.controls.drive.DriveControls;
 import frc.robot.subsystems.controls.elevator.ElevatorControls;
 import frc.robot.subsystems.implementations.drive.DriveBase;
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 2349ade1..a995797c 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -1,7 +1,5 @@
 package frc.robot.config.game.reefscape2025;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
@@ -13,8 +11,6 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
-import frc.robot.commands.common.arm.ArmToPosition;
-import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.io.implementations.motor.MotorIOBase.MotorIOBaseSettings;
 import frc.robot.io.implementations.motor.MotorIOTalonFx;
 import frc.robot.io.implementations.motor.MotorIOTalonFx.TalonFxSettings;
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
index faacc93e..acdedeb8 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
@@ -1,7 +1,5 @@
 package frc.robot.config.game.reefscape2025;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
@@ -13,8 +11,6 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
-import frc.robot.commands.common.arm.ArmToPosition;
-import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.io.implementations.motor.MotorIOBase.MotorIOBaseSettings;
 import frc.robot.io.implementations.motor.MotorIOSparkMax;
 import frc.robot.io.implementations.motor.MotorIOSparkMax.SparkMaxSettings;
diff --git a/src/main/java/frc/robot/io/interfaces/DriveIO.java b/src/main/java/frc/robot/io/interfaces/DriveIO.java
index 7eac4ea7..c85bb811 100644
--- a/src/main/java/frc/robot/io/interfaces/DriveIO.java
+++ b/src/main/java/frc/robot/io/interfaces/DriveIO.java
@@ -11,7 +11,6 @@ public static class DriveIOInputs {
     public double poseX = 0.0;
     public double poseY = 0.0;
     public double poseRotInDegrees = 0.0;
-    public double distanceFromSpeaker = 0;
   }
 
   /** Updates the set of loggable inputs. */
@@ -20,20 +19,6 @@ public void updateInputs(DriveIOInputs inputs, SwerveDrive swerveDrive) {
     inputs.poseX = inputs.pose.getTranslation().getX();
     inputs.poseY = inputs.pose.getTranslation().getY();
     inputs.poseRotInDegrees = inputs.pose.getRotation().getDegrees();
-    /*
-        if (!DevilBotState.isRedAlliance()) {
-          inputs.distanceFromSpeaker =
-              Math.sqrt(
-                  Math.pow(inputs.poseX - DriveBase.Constants.blueSpeakerX, 2)
-                      + Math.pow(inputs.poseY - DriveBase.Constants.speakerY, 2));
-        } else {
-          inputs.distanceFromSpeaker =
-              Math.sqrt(
-                  Math.pow(inputs.poseX - DriveBase.Constants.redSpeakerX, 2)
-                      + Math.pow(inputs.poseY - DriveBase.Constants.speakerY, 2));
-        }
-        ;
-    */
   }
   // Other methods for controlling the drive subsystem...
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index b748e1c4..689f49cc 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -13,31 +13,19 @@ public enum TargetPose {
   ORIGIN(0, "O", "Origin", new Pose2d(0.0, 0.0, new Rotation2d(0)), 0),
   FEEDER_R(1, "FR", "Feeder Right", new Pose2d(1.05, 1, new Rotation2d(50)), 0),
   FEEDER_L(2, "FL", "Feeder Left", new Pose2d(1.05, 7, Rotation2d.fromDegrees(-50)), 0),
-  REEF_A(3, "A", "Reef A", new Pose2d(3.15, 4.28, Rotation2d.fromDegrees(0)), 
-  -1),
+  REEF_A(3, "A", "Reef A", new Pose2d(3.15, 4.28, Rotation2d.fromDegrees(0)), -1),
   REEF_G(9, "G", "Reef G", new Pose2d(5.5, 3.95, Rotation2d.fromDegrees(180)), -1),
   PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0),
-  REEF_B(4, "B", "Reef B", new Pose2d(3.15, 3.70, Rotation2d.fromDegrees(0)), 
-  0),
-  REEF_C(5, "C", "Reef C", new Pose2d(3.60, 2.93, Rotation2d.fromDegrees(60)), 
-  -1),
-  REEF_D(6, "D", "Reef D", new Pose2d(3.96, 2.74, Rotation2d.fromDegrees(60)), 
-  0),
-  REEF_E(7, "E", "Reef E", new Pose2d(5.10, 2.75, Rotation2d.fromDegrees(124)), 
-  -1),
-  REEF_F(8, "F", "Reef F", new Pose2d(5.36, 2.93, Rotation2d.fromDegrees(124)), 
-  0),
-  REEF_H(10, "H", "Reef H", new Pose2d(5.80, 4.20, Rotation2d.fromDegrees(180)), 
-  0),
-  REEF_I(11, "I", "Reef I", new Pose2d(5.30, 5.15, Rotation2d.fromDegrees(-124)), 
-  -1),
-  REEF_J(12, "J", "Reef J", new Pose2d(5.00, 5.30, Rotation2d.fromDegrees(-124)), 
-  0),
-  REEF_K(13, "K", "Reef K", new Pose2d(4.00, 5.33, Rotation2d.fromDegrees(-60)), 
--1),
-  REEF_L(14, "L", "Reef L", new Pose2d(3.70, 5.20, Rotation2d.fromDegrees(-60)), 
-  0);
-  
+  REEF_B(4, "B", "Reef B", new Pose2d(3.15, 3.70, Rotation2d.fromDegrees(0)), 0),
+  REEF_C(5, "C", "Reef C", new Pose2d(3.60, 2.93, Rotation2d.fromDegrees(60)), -1),
+  REEF_D(6, "D", "Reef D", new Pose2d(3.96, 2.74, Rotation2d.fromDegrees(60)), 0),
+  REEF_E(7, "E", "Reef E", new Pose2d(5.10, 2.75, Rotation2d.fromDegrees(124)), -1),
+  REEF_F(8, "F", "Reef F", new Pose2d(5.36, 2.93, Rotation2d.fromDegrees(124)), 0),
+  REEF_H(10, "H", "Reef H", new Pose2d(5.80, 4.20, Rotation2d.fromDegrees(180)), 0),
+  REEF_I(11, "I", "Reef I", new Pose2d(5.30, 5.15, Rotation2d.fromDegrees(-124)), -1),
+  REEF_J(12, "J", "Reef J", new Pose2d(5.00, 5.30, Rotation2d.fromDegrees(-124)), 0),
+  REEF_K(13, "K", "Reef K", new Pose2d(4.00, 5.33, Rotation2d.fromDegrees(-60)), -1),
+  REEF_L(14, "L", "Reef L", new Pose2d(3.70, 5.20, Rotation2d.fromDegrees(-60)), 0);
 
   private int index;
   private String shortName;
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPoseOLD.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPoseOLD.java
new file mode 100644
index 00000000..3b7e1d09
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPoseOLD.java
@@ -0,0 +1,127 @@
+package frc.robot.subsystems.controls.drive;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+public enum TargetPoseOLD {
+  ORIGIN(
+      0,
+      "O",
+      "Origin",
+      new Pose2d(0.0, 0.0, new Rotation2d(0)),
+      new Pose2d(0.0, 0.0, new Rotation2d(0)),
+      0),
+  FEEDER_R(
+      1,
+      "FR",
+      "Feeder Right",
+      new Pose2d(16.44, 7.25, new Rotation2d(230)),
+      new Pose2d(1.05, 1, new Rotation2d(50)),
+      0),
+  FEEDER_L(
+      2,
+      "FL",
+      "Feeder Left",
+      new Pose2d(16.02, 1, Rotation2d.fromDegrees(-230)),
+      new Pose2d(1.05, 7, Rotation2d.fromDegrees(-50)),
+      0),
+  REEF_A(
+      3,
+      "A",
+      "Reef A",
+      new Pose2d(14.425, 4.175, Rotation2d.fromDegrees(180)),
+      new Pose2d(3.25, 4.05, Rotation2d.fromDegrees(0)),
+      -1),
+  REEF_G(
+      9,
+      "G",
+      "Reef G",
+      new Pose2d(11.5, 4.175, Rotation2d.fromDegrees(0)),
+      new Pose2d(5.5, 3.95, Rotation2d.fromDegrees(180)),
+      -1),
+  PROCESSOR(
+      15,
+      "P",
+      "Processor",
+      new Pose2d(11.5, 7.3, new Rotation2d(90)),
+      new Pose2d(6, 0.75, new Rotation2d(270)),
+      0);
+
+  private int index;
+  private String shortName;
+  private String longName;
+  private Pose2d redPose;
+  private Pose2d bluePose;
+
+  private Pose2d redPrepPose;
+  private Pose2d redEndPose;
+  private Pose2d bluePrepPose;
+  private Pose2d blueEndPose;
+
+  // NOTE differential poses ~= 0.5m away from target
+
+  public int getIndex() {
+    return this.index;
+  }
+
+  public String getShortName() {
+    return this.shortName;
+  }
+
+  public String getLongName() {
+    return this.longName;
+  }
+
+  public Pose2d getPose(boolean isRed) {
+    return (isRed) ? this.redPose : this.bluePose;
+  }
+
+  public Pose2d getMyPose() {
+    return this.bluePose;
+  }
+
+  public Pose2d getMyPrepPose() {
+    return this.bluePrepPose;
+  }
+
+  public Pose2d getPrepPose(boolean isRed) {
+    return (isRed) ? this.redPrepPose : this.bluePrepPose;
+  }
+
+  public Pose2d getEndPose(boolean isRed) {
+    return (isRed) ? this.redEndPose : this.blueEndPose;
+  }
+
+  private void calcPrepPoses(boolean isReefStickLeft) {
+    double distance = isReefStickLeft ? -0.5 : 0.5;
+    this.redPrepPose =
+        this.redPose.transformBy(new Transform2d(new Translation2d(0, distance), new Rotation2d()));
+    this.bluePrepPose =
+        this.bluePose.transformBy(
+            new Transform2d(new Translation2d(0, distance), new Rotation2d()));
+  }
+
+  private void calcEndPoses() {
+    this.redEndPose =
+        this.redPose.transformBy(new Transform2d(new Translation2d(-0.5, 0), new Rotation2d()));
+    this.blueEndPose =
+        this.bluePose.transformBy(new Transform2d(new Translation2d(-0.5, 0), new Rotation2d()));
+  }
+
+  private TargetPoseOLD(
+      int idx, String sName, String lName, Pose2d redPose, Pose2d bluePose, int reefPosition) {
+    this.index = idx;
+    this.shortName = sName;
+    this.longName = lName;
+    this.redPose = redPose;
+    this.bluePose = bluePose;
+
+    // If this pose is not on the reef, skip
+    if (reefPosition == 0) return;
+
+    calcPrepPoses(reefPosition < 0);
+    calcEndPoses();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index 0692b601..2342f982 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -6,15 +6,12 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.interfaces.Vision;
 import java.util.ArrayList;
-import java.util.LinkedList;
 import java.util.List;
 import java.util.Optional;
 import java.util.function.Supplier;
-import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
@@ -54,6 +51,7 @@ private VisionCameraImpl(Camera camera, AprilTagFieldLayout fieldLayout) {
     }
 
     private void update() {
+      // This methods is invoked "periodically"
       for (PhotonPipelineResult photonPipelineResult : camera.getAllUnreadResults()) {
         estimatedRobotPose = poseEstimator.update(photonPipelineResult);
         result = photonPipelineResult;
@@ -100,24 +98,13 @@ private int getIndex() {
     private String getName() {
       return camera.getName();
     }
-
-    private Transform3d getRobotToCamera() {
-      return robotToCamera;
-    }
   }
 
   private final List<VisionCameraImpl> cameras = new ArrayList<VisionCameraImpl>();
-  private VisionCameraImpl primaryCamera = null;
   private final AprilTagFieldLayout fieldLayout;
   private VisionMeasurementConsumer visionMeasurementConsumer = null;
   List<Camera> visionCameras = new ArrayList<Camera>();
 
-  /* Debug Info */
-  @AutoLogOutput private int debugTargetsVisible;
-
-  @AutoLogOutput private double debugTargetDistance = 0;
-  @AutoLogOutput private double debugTargetYaw = 0;
-
   /* Simulation Support*/
   private boolean simEnabled = false;
   private VisionSystemSim simVision;
@@ -131,10 +118,6 @@ public VisionSubsystem(AprilTagFieldLayout fieldLayout) {
   public void addCamera(Camera camera) {
     cameras.add(new VisionCameraImpl(camera, fieldLayout));
     visionCameras.add(camera);
-
-    if (1 == cameras.size()) {
-      primaryCamera = this.cameras.get(0);
-    }
   }
 
   @Override
@@ -164,8 +147,6 @@ public void enableSimulation(Supplier<Pose2d> poseSupplier, boolean enableWireFr
   }
 
   public void updatePoseEstimation() {
-    // Initialize new list of robot poses (for debugging)
-    List<Pose2d> debugRobotPoses = new LinkedList<>();
 
     if (simEnabled) {
       simVision.update(simPoseSupplier.get());
@@ -174,41 +155,35 @@ public void updatePoseEstimation() {
     for (VisionCameraImpl camera : cameras) {
       camera.update();
 
+      // Default values: in case no targets are seen
+      double distanceToTarget = -1;
+      Pose2d estimatedRobotPoseFromCamera = new Pose2d(-10.0, -10.0, new Rotation2d(0));
+
       Optional<EstimatedRobotPose> currentEstimatedRobotPose = camera.getEstimatedRobotPose();
       if (currentEstimatedRobotPose.isPresent()) {
-        debugTargetDistance = camera.getDistanceToBestTarget();
-        double targetDistance;
-        if (RobotState.isAutonomous()) {
-          targetDistance = 5.0;
-        } else {
-          targetDistance = 0.8;
-        }
+
+        estimatedRobotPoseFromCamera = currentEstimatedRobotPose.get().estimatedPose.toPose2d();
+        distanceToTarget = camera.getDistanceToBestTarget();
+
         // Add vision measurement to the consumer.
-        if (visionMeasurementConsumer != null && debugTargetDistance < targetDistance) {
+        if (visionMeasurementConsumer != null && distanceToTarget < 2) {
           visionMeasurementConsumer.add(
-              currentEstimatedRobotPose.get().estimatedPose.toPose2d(),
+              estimatedRobotPoseFromCamera,
               currentEstimatedRobotPose.get().timestampSeconds,
-              VecBuilder.fill(
-                  debugTargetDistance / 2, debugTargetDistance / 2, debugTargetDistance / 2));
+              VecBuilder.fill(distanceToTarget / 2, distanceToTarget / 2, distanceToTarget / 2));
         }
         /* NOTE standard deviation format:
          * (x position in meters, y position in meters, and heading in radians)
          * Increase these numbers to trust the vision pose measurement less.
          */
-
-        // Log estimated robot pose for debugging
-        debugRobotPoses.add(currentEstimatedRobotPose.get().estimatedPose.toPose2d());
-      } else {
-        // Display the robot pose "out of the arena" (indicating no pose found)
-        debugRobotPoses.add(new Pose2d(-10.0, -10.0, new Rotation2d(0)));
       }
-    }
 
-    // Record estimated robot pose to AdvantageKit networktables
-    if (debugRobotPoses.size() > 0) {
+      // Log information for debugging
       Logger.recordOutput(
-          "VisionSubsystem/DEBUGestimatedCameraPoses",
-          debugRobotPoses.toArray(new Pose2d[debugRobotPoses.size()]));
+          "VisionSubsystem/Distances To Target/" + camera.getName(), distanceToTarget);
+      Logger.recordOutput(
+          "VisionSubsystem/Estimated Robot Poses/" + camera.getName(),
+          estimatedRobotPoseFromCamera);
     }
   }
 
@@ -217,82 +192,6 @@ public void periodic() {
     updatePoseEstimation();
   }
 
-  private PhotonTrackedTarget findAprilTag(int id) {
-    if (primaryCamera != null) {
-      for (PhotonTrackedTarget target : primaryCamera.getTargets()) {
-        if (target.getFiducialId() == id) {
-          return target;
-        }
-      }
-    }
-    return null;
-  }
-
-  @Override
-  public Optional<Double> getDistanceToAprilTag(int id) {
-    PhotonTrackedTarget target = findAprilTag(id);
-
-    if ((target != null) && (primaryCamera != null)) {
-      return Optional.of(
-          PhotonUtils.calculateDistanceToTargetMeters(
-                  primaryCamera.getRobotToCamera().getZ(),
-                  fieldLayout.getTagPose(target.getFiducialId()).get().getZ(),
-                  -primaryCamera.getRobotToCamera().getRotation().getY(),
-                  Units.degreesToRadians(target.getPitch()))
-              + Constants.visionDistanceOffsetInMeters);
-    }
-    return Optional.empty();
-  }
-
-  @Override
-  public Optional<Integer> getBestTargetId() {
-    if ((primaryCamera != null) && (primaryCamera.hasTargets())) {
-      return Optional.of(primaryCamera.getBestTarget().getFiducialId());
-    }
-    return Optional.empty();
-  }
-
-  @Override
-  public Optional<Double> getYawToBestTarget() {
-    if ((primaryCamera != null) && (primaryCamera.hasTargets())) {
-      return Optional.of(primaryCamera.getBestTarget().getYaw());
-    }
-    return Optional.empty();
-  }
-
-  @Override
-  public Optional<Double> getDistanceToBestTarget() {
-    if ((primaryCamera != null) && (primaryCamera.hasTargets())) {
-      return getDistanceToAprilTag(primaryCamera.getBestTarget().getFiducialId());
-    }
-    return Optional.empty();
-  }
-
-  @Override
-  public Optional<Double> getYawToAprilTag(int id) {
-    PhotonTrackedTarget target = findAprilTag(id);
-
-    if (target != null) {
-      return Optional.of(target.getYaw());
-    }
-
-    return Optional.empty();
-  }
-
-  @Override
-  public boolean setPrimaryCamera(String name) {
-    boolean foundCamera = false;
-
-    for (VisionCameraImpl camera : cameras) {
-      if (camera.getName().equals(name)) {
-        primaryCamera = camera;
-        foundCamera = true;
-        break;
-      }
-    }
-    return foundCamera;
-  }
-
   @Override
   public void setVisionMeasurementConsumer(VisionMeasurementConsumer func) {
     visionMeasurementConsumer = func;
diff --git a/src/main/java/frc/robot/subsystems/interfaces/Vision.java b/src/main/java/frc/robot/subsystems/interfaces/Vision.java
index 129c2c7e..d60a046d 100644
--- a/src/main/java/frc/robot/subsystems/interfaces/Vision.java
+++ b/src/main/java/frc/robot/subsystems/interfaces/Vision.java
@@ -7,7 +7,6 @@
 import edu.wpi.first.math.numbers.N3;
 import java.text.DecimalFormat;
 import java.util.List;
-import java.util.Optional;
 import java.util.function.Supplier;
 
 public interface Vision {
@@ -108,38 +107,6 @@ public String toString() {
     }
   }
 
-  public default boolean setPrimaryCamera(String name) {
-    return true;
-  }
-
-  public Optional<Integer> getBestTargetId();
-
-  public Optional<Double> getDistanceToBestTarget();
-
-  /**
-   * Returns the yaw in degrees to the best target (relative to the primary camera)
-   *
-   * @return yaw to the best target (in degrees)
-   */
-  public Optional<Double> getYawToBestTarget();
-
-  /**
-   * Returns the distance to the specified april tag in meters (relative to the primary camera)
-   *
-   * @param id AprilTag ID
-   * @return distance to the specified april tag (in meters).
-   */
-  public Optional<Double> getDistanceToAprilTag(int id);
-
-  /**
-   * Returns the yaw in degrees to the specified april tag in meters (relative to the primary
-   * camera)
-   *
-   * @param id AprilTag ID
-   * @return yaw to the specified april tag (in degrees).
-   */
-  public Optional<Double> getYawToAprilTag(int id);
-
   public default void enableSimulation(Supplier<Pose2d> poseSupplier, boolean enableWireFrame) {}
 
   public default void setVisionMeasurementConsumer(VisionMeasurementConsumer func) {}

From 323bf7843fd7636ce773761ac8d6363493332cf5 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Thu, 6 Mar 2025 19:18:29 -0500
Subject: [PATCH 30/90] update elastic

---
 src/main/deploy/elastic-layout.json | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json
index 6b001f6d..4a536fc5 100644
--- a/src/main/deploy/elastic-layout.json
+++ b/src/main/deploy/elastic-layout.json
@@ -160,7 +160,7 @@
             "height": 128.0,
             "type": "Split Button Chooser",
             "properties": {
-              "topic": "/SmartDashboard/Driver /Commands/Prepare To Score Chooser",
+              "topic": "/SmartDashboard/Driver /Misc/Prepare To Score Chooser",
               "period": 0.06
             }
           },
@@ -211,7 +211,7 @@
             "height": 128.0,
             "type": "Large Text Display",
             "properties": {
-              "topic": "/SmartDashboard/Driver /Commands/Prepare To Score Selection",
+              "topic": "/SmartDashboard/Driver /Misc/Prepare To Score Selection",
               "period": 0.06,
               "data_type": "double"
             }

From a28890b83403f9fae35b0e63237d320934ec070f Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Thu, 6 Mar 2025 20:36:50 -0500
Subject: [PATCH 31/90] fixing positions

---
 .../subsystems/controls/drive/TargetPose.java    | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 689f49cc..096be191 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -14,18 +14,18 @@ public enum TargetPose {
   FEEDER_R(1, "FR", "Feeder Right", new Pose2d(1.05, 1, new Rotation2d(50)), 0),
   FEEDER_L(2, "FL", "Feeder Left", new Pose2d(1.05, 7, Rotation2d.fromDegrees(-50)), 0),
   REEF_A(3, "A", "Reef A", new Pose2d(3.15, 4.28, Rotation2d.fromDegrees(0)), -1),
-  REEF_G(9, "G", "Reef G", new Pose2d(5.5, 3.95, Rotation2d.fromDegrees(180)), -1),
-  PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0),
-  REEF_B(4, "B", "Reef B", new Pose2d(3.15, 3.70, Rotation2d.fromDegrees(0)), 0),
+  REEF_B(4, "B", "Reef B", new Pose2d(3.15, 3.70, Rotation2d.fromDegrees(0)), 1),
   REEF_C(5, "C", "Reef C", new Pose2d(3.60, 2.93, Rotation2d.fromDegrees(60)), -1),
-  REEF_D(6, "D", "Reef D", new Pose2d(3.96, 2.74, Rotation2d.fromDegrees(60)), 0),
+  REEF_D(6, "D", "Reef D", new Pose2d(3.96, 2.74, Rotation2d.fromDegrees(60)), 1),
   REEF_E(7, "E", "Reef E", new Pose2d(5.10, 2.75, Rotation2d.fromDegrees(124)), -1),
-  REEF_F(8, "F", "Reef F", new Pose2d(5.36, 2.93, Rotation2d.fromDegrees(124)), 0),
-  REEF_H(10, "H", "Reef H", new Pose2d(5.80, 4.20, Rotation2d.fromDegrees(180)), 0),
+  REEF_F(8, "F", "Reef F", new Pose2d(5.36, 2.93, Rotation2d.fromDegrees(124)), 1),
+  REEF_G(9, "G", "Reef G", new Pose2d(5.5, 3.95, Rotation2d.fromDegrees(180)), -1),
+  REEF_H(10, "H", "Reef H", new Pose2d(5.80, 4.20, Rotation2d.fromDegrees(180)), 1),
   REEF_I(11, "I", "Reef I", new Pose2d(5.30, 5.15, Rotation2d.fromDegrees(-124)), -1),
-  REEF_J(12, "J", "Reef J", new Pose2d(5.00, 5.30, Rotation2d.fromDegrees(-124)), 0),
+  REEF_J(12, "J", "Reef J", new Pose2d(5.00, 5.30, Rotation2d.fromDegrees(-124)), 1),
   REEF_K(13, "K", "Reef K", new Pose2d(4.00, 5.33, Rotation2d.fromDegrees(-60)), -1),
-  REEF_L(14, "L", "Reef L", new Pose2d(3.70, 5.20, Rotation2d.fromDegrees(-60)), 0);
+  REEF_L(14, "L", "Reef L", new Pose2d(3.65, 5.18, Rotation2d.fromDegrees(-60)), 1),
+  PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0);
 
   private int index;
   private String shortName;

From 98b6221f777de9b48818c5d24f617aca9717c61c Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Fri, 7 Mar 2025 11:50:58 -0500
Subject: [PATCH 32/90] Fix branch so that it builds

---
 .../java/frc/robot/config/game/reefscape2025/RobotConfig.java   | 2 +-
 .../robot/subsystems/controls/combination/DriverControls.java   | 1 +
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index 559cb05b..faeb0e1c 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -210,7 +210,7 @@ public void configureBindings() {
     // Send vision-based odometry measurements to drive's odometry calculations
     vision.setVisionMeasurementConsumer(drive::addVisionMeasurement);
 
-    DriveControls.setupController(drive, mainController);
+    DriveControls.setupController(drive, elevator, coralArm, mainController);
     DriverAssistControls.setupController(elevator, coralArm, climberArm, assistController);
     DriverControls.setupController(elevator, coralArm, climberArm, mainController);
     PitControls.setupPitControls(elevator, coralArm, climberArm);
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 1f5910ba..5448a4a7 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;

From 72f1d921991bf34feb47b97b7184d01cd7ea32f0 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Fri, 7 Mar 2025 12:57:30 -0500
Subject: [PATCH 33/90] Implement "ad-hoc reverse" command

---
 .../common/drive/DriveToPositionX.java        | 75 +++++++++++++++++++
 .../subsystems/controls/drive/TargetPose.java |  7 +-
 .../drive/DriveSwerveYAGSL.java               |  4 -
 3 files changed, 77 insertions(+), 9 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java

diff --git a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
new file mode 100644
index 00000000..8d5d2fdc
--- /dev/null
+++ b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
@@ -0,0 +1,75 @@
+package frc.robot.commands.common.drive;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.Constants;
+import frc.robot.subsystems.implementations.drive.DriveBase;
+import frc.robot.subsystems.interfaces.Drive;
+import java.util.function.DoubleSupplier;
+
+public class DriveToPositionX extends Command {
+  Drive drive;
+  DoubleSupplier distanceMeters;
+  Pose2d startingPose, targetPose;
+  // HACK: manually copied from Swerve subsystem translation PID
+  PIDController positionPID = new PIDController(7.5, 0.0, 0.0);
+  Timer timer = new Timer();
+
+  public DriveToPositionX(Drive drive, DoubleSupplier distanceMeters) {
+    this.drive = drive;
+    this.distanceMeters = distanceMeters;
+
+    // HACK: not sure what a good tolerance would be...
+    positionPID.setTolerance(0.25);
+    addRequirements((Subsystem) drive);
+  }
+
+  @Override
+  public void initialize() {
+    double d = this.distanceMeters.getAsDouble();
+    targetPose = drive.getPose().transformBy(new Transform2d(d, 0, new Rotation2d()));
+    positionPID.reset();
+    positionPID.setSetpoint(d);
+    timer.reset();
+    if (Constants.debugCommands) {
+      System.out.println(
+          "START: " + this.getClass().getSimpleName() + " distance: " + d + " currentPosition: 0");
+    }
+  }
+
+  @Override
+  public void execute() {
+    double linearSpeed = positionPID.calculate(targetPose.relativeTo(drive.getPose()).getX());
+    drive.runVelocity(new ChassisSpeeds(linearSpeed * drive.getMaxLinearSpeed(), 0, 0));
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (positionPID.atSetpoint()) {
+      if (timer.get() >= DriveBase.Constants.pidSettlingTimeInSeconds) {
+        return true;
+      }
+    } else {
+      timer.reset();
+    }
+    return false;
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    if (interrupted) {
+      System.err.println("INTERRUPTED: " + this.getClass().getSimpleName());
+    }
+
+    drive.runVelocity(new ChassisSpeeds());
+    if (Constants.debugCommands) {
+      System.out.println("  END: " + this.getClass().getSimpleName());
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 096be191..54c61978 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -3,7 +3,6 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
 
 public enum TargetPose {
   // NOTE: WE ARE IDENTIFYING THESE POSITIONS ON THE BLUE SIDE OF THE FIELD!!!
@@ -75,10 +74,8 @@ private TargetPose(int idx, String sName, String lName, Pose2d targetPose, int r
       this.endPose = null;
     } else {
       double distance = (reefPosition < 0) ? -0.7 : 0.7;
-      this.prepPose =
-          targetPose.transformBy(new Transform2d(new Translation2d(0, distance), new Rotation2d()));
-      this.endPose =
-          targetPose.transformBy(new Transform2d(new Translation2d(0, 0), new Rotation2d()));
+      this.prepPose = targetPose.transformBy(new Transform2d(0, distance, new Rotation2d()));
+      this.endPose = targetPose.transformBy(new Transform2d(-0.7, 0, new Rotation2d()));
     }
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index a178ceda..f97bca51 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -38,10 +38,6 @@ public static class Constants {
   @AutoLogOutput private boolean fieldOrientedDrive = false;
   private PIDConstants translationPIDConstants =
       new PIDConstants(7.5, 0.0, 0.0); // Translation PID constants
-  // private PIDConstants rotationPIDConstants =
-  //     new PIDConstants(8.0, 0.0, 1.0); // Rotation PID constants
-  // private PIDConstants translationPIDConstants =
-  //     new PIDConstants(7.0, 0.0, 0.3); // Translation PID constants
   private PIDConstants rotationPIDConstants =
       new PIDConstants(6.0, 0.0, 0.0); // Rotation PID constants
 

From 1580abfb10ce091d3f2811a3296eee816928309c Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Fri, 7 Mar 2025 13:25:57 -0500
Subject: [PATCH 34/90] Fixed reverse drive command

---
 .../common/drive/DriveToPositionX.java        | 30 ++-----------------
 1 file changed, 3 insertions(+), 27 deletions(-)

diff --git a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
index 8d5d2fdc..6b07e923 100644
--- a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
+++ b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
@@ -1,32 +1,23 @@
 package frc.robot.commands.common.drive;
 
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.robot.Constants;
-import frc.robot.subsystems.implementations.drive.DriveBase;
 import frc.robot.subsystems.interfaces.Drive;
 import java.util.function.DoubleSupplier;
 
 public class DriveToPositionX extends Command {
   Drive drive;
   DoubleSupplier distanceMeters;
-  Pose2d startingPose, targetPose;
-  // HACK: manually copied from Swerve subsystem translation PID
-  PIDController positionPID = new PIDController(7.5, 0.0, 0.0);
-  Timer timer = new Timer();
+  Pose2d targetPose;
 
   public DriveToPositionX(Drive drive, DoubleSupplier distanceMeters) {
     this.drive = drive;
     this.distanceMeters = distanceMeters;
-
-    // HACK: not sure what a good tolerance would be...
-    positionPID.setTolerance(0.25);
     addRequirements((Subsystem) drive);
   }
 
@@ -34,31 +25,16 @@ public DriveToPositionX(Drive drive, DoubleSupplier distanceMeters) {
   public void initialize() {
     double d = this.distanceMeters.getAsDouble();
     targetPose = drive.getPose().transformBy(new Transform2d(d, 0, new Rotation2d()));
-    positionPID.reset();
-    positionPID.setSetpoint(d);
-    timer.reset();
-    if (Constants.debugCommands) {
-      System.out.println(
-          "START: " + this.getClass().getSimpleName() + " distance: " + d + " currentPosition: 0");
-    }
   }
 
   @Override
   public void execute() {
-    double linearSpeed = positionPID.calculate(targetPose.relativeTo(drive.getPose()).getX());
-    drive.runVelocity(new ChassisSpeeds(linearSpeed * drive.getMaxLinearSpeed(), 0, 0));
+    drive.runVelocity(new ChassisSpeeds(-drive.getMaxLinearSpeed(), 0, 0));
   }
 
   @Override
   public boolean isFinished() {
-    if (positionPID.atSetpoint()) {
-      if (timer.get() >= DriveBase.Constants.pidSettlingTimeInSeconds) {
-        return true;
-      }
-    } else {
-      timer.reset();
-    }
-    return false;
+    return targetPose.relativeTo(drive.getPose()).getX() > 0;
   }
 
   @Override

From 58f2f70eb1425fcc25e177c43989355713d34e7d Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Fri, 7 Mar 2025 13:47:17 -0500
Subject: [PATCH 35/90] attempting to manually mitigate out-of-scope changes

---
 .../game/reefscape2025/RobotConfig.java       |   2 +-
 .../game/reefscape2025/RobotConfigNemo.java   |   2 +-
 .../reefscape2025/RobotConfigPhoenix.java     |  12 --
 .../controls/drive/DriveControls.java         | 108 ++----------------
 .../drive/DriveSwerveYAGSL.java               |   8 +-
 5 files changed, 11 insertions(+), 121 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index faeb0e1c..559cb05b 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -210,7 +210,7 @@ public void configureBindings() {
     // Send vision-based odometry measurements to drive's odometry calculations
     vision.setVisionMeasurementConsumer(drive::addVisionMeasurement);
 
-    DriveControls.setupController(drive, elevator, coralArm, mainController);
+    DriveControls.setupController(drive, mainController);
     DriverAssistControls.setupController(elevator, coralArm, climberArm, assistController);
     DriverControls.setupController(elevator, coralArm, climberArm, mainController);
     PitControls.setupPitControls(elevator, coralArm, climberArm);
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
index d4e898ad..2ade8ccf 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
@@ -104,7 +104,7 @@ public RobotConfigNemo() {
       CoralArmControls.Constants.autoZeroSettings.minResetCurrent = 0.5;
       CoralArmControls.Constants.autoZeroSettings.resetPositionRad =
           Units.degreesToRadians(
-              armSettings.minAngleInDegrees - 10); // We have an offest about 15 degrees
+              armSettings.minAngleInDegrees); // We have an offest about 15 degrees
       CoralArmControls.Constants.autoZeroSettings.initialReverseDuration =
           1.0; // Set the seconds of reverse before zero. Set to zero if there shound be no reverse
 
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
index 154c4ecf..7b8feaf9 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
@@ -1,15 +1,11 @@
 package frc.robot.config.game.reefscape2025;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
-import frc.robot.commands.common.arm.ArmToPosition;
-import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.subsystems.implementations.drive.DriveBase;
 import frc.robot.subsystems.implementations.drive.DriveSwerveYAGSL;
 import frc.robot.subsystems.interfaces.Drive;
@@ -34,13 +30,5 @@ public RobotConfigPhoenix() {
             new Transform3d(
                 new Translation3d(Units.inchesToMeters(13.25), 0, Units.inchesToMeters(6.25)),
                 new Rotation3d(0, -Units.degreesToRadians(15), 0))));
-
-    NamedCommands.registerCommand(
-        "Move Elevator", new ElevatorToPosition(RobotConfig.elevator, () -> 1.8288));
-    NamedCommands.registerCommand(
-        "Move Arm High", new ArmToPosition(RobotConfig.coralArm, () -> 135.0));
-    NamedCommands.registerCommand(
-        "Move Arm Low", new ArmToPosition(RobotConfig.coralArm, () -> 90.0));
-    autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 20205c73..d618cd9e 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -9,17 +9,11 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.drive.DriveCommand;
-import frc.robot.commands.common.elevator.ElevatorToPosition;
-import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Drive;
-import frc.robot.subsystems.interfaces.Elevator;
 import java.util.Map;
 
 public class DriveControls {
@@ -29,19 +23,7 @@ private enum TargetPoseOption {
     FEEDER_2(2),
     PROCESSOR(3),
     REEF_A(4),
-    REEF_G(10),
-    REEF_C(6),
-    REEF_D(7),
-    REEF_L(15),
-    // WOW_Test(9),
-    // WOW_Test1(10),
-    REEF_B(5),
-    REEF_E(8),
-    REEF_F(9),
-    REEF_H(11),
-    REEF_I(12),
-    REEF_J(13),
-    REEF_K(14);
+    REEF_G(5);
 
     private int index;
 
@@ -57,8 +39,7 @@ private TargetPoseOption(int idx) {
 
   protected static int myCoolPoseKeyIdx = 0;
 
-  public static void setupController(
-      Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
+  public static void setupController(Drive drive, CommandXboxController controller) {
     SubsystemBase driveSubsystem = (SubsystemBase) drive;
     driveSubsystem.setDefaultCommand(
         new DriveCommand(
@@ -97,30 +78,15 @@ public static void setupController(
         poseReefA = new Pose2d(15, 4.175, Rotation2d.fromDegrees(180)),
         poseReefAClose = new Pose2d(14.425, 4.175, Rotation2d.fromDegrees(180)),
         poseReefG = new Pose2d(11.57, 4.17, Rotation2d.fromDegrees(0)),
-        poseReefGClose = new Pose2d(11.5, 4.175, Rotation2d.fromDegrees(0)),
-        poseReefC = new Pose2d(14.17, 5.25, Rotation2d.fromDegrees(-124)),
-        poseReefD = new Pose2d(13.63, 5.56, Rotation2d.fromDegrees(-124)),
-        poseReefL = new Pose2d(14.14, 2.80, Rotation2d.fromDegrees(124)),
-        // poseTest = new Pose2d(2.50, 5.57, Rotation2d.fromDegrees(-124)),
-        // poseTest2 = new Pose2d(2.46, 2.73, Rotation2d.fromDegrees(-124)),
-        poseReefB = new Pose2d(14.59, 4.34, Rotation2d.fromDegrees(180)),
-        poseReefE = new Pose2d(12.46, 5.52, Rotation2d.fromDegrees(-60)),
-        poseReefF = new Pose2d(12.00, 5.25, Rotation2d.fromDegrees(-60)),
-        poseReefHStart = new Pose2d(11.00, 3.92, Rotation2d.fromDegrees(-1)),
-        poseReefHEnd = new Pose2d(11.76, 3.92, Rotation2d.fromDegrees(-1)),
-        poseReefI = new Pose2d(12.00, 2.75, Rotation2d.fromDegrees(60)),
-        poseReefJ = new Pose2d(12.50, 2.50, Rotation2d.fromDegrees(60)),
-        poseReefK = new Pose2d(13.65, 2.50, Rotation2d.fromDegrees(120));
+        poseReefGClose = new Pose2d(11.5, 4.175, Rotation2d.fromDegrees(0));
     //  PathConstraints constraints = new PathConstraints(4.9672, 9.3664784, 2 * Math.PI, 4 *
     //  Math.PI);
     // PathConstraints constraints = new PathConstraints(2, 1.5, 2 * Math.PI, 4 * Math.PI);
-    // PathConstraints constraints = new PathConstraints(4.5, 1.5, 2 * Math.PI, Math.PI / 4);
+    // PathConstraints constraints = new PathConstraints(0.5, 4.5, Math.PI / 4, 4 * Math.PI);
     PathConstraints constraints =
         new PathConstraints(
             drive.getMaxLinearSpeed(), 1.5, drive.getMaxAngularSpeed(), Math.PI / 4);
 
-    // PathConstraints constraints = new PathConstraints(0.5, 4.5, Math.PI / 4, 4 * Math.PI);
-
     // Temporary UI to allow user to modify destination on-the-fly
     SendableChooser<TargetPoseOption> chooser = new SendableChooser<>();
     chooser.setDefaultOption("Origin", TargetPoseOption.ORIGIN);
@@ -129,18 +95,6 @@ public static void setupController(
     chooser.addOption("Processor", TargetPoseOption.PROCESSOR);
     chooser.addOption("Reef A", TargetPoseOption.REEF_A);
     chooser.addOption("Reef G", TargetPoseOption.REEF_G);
-    chooser.addOption("Reef C", TargetPoseOption.REEF_C);
-    chooser.addOption("Reef D", TargetPoseOption.REEF_D);
-    // chooser.addOption("Reef L", TargetPoseOption.REEF_L);
-    // chooser.addOption("Test", TargetPoseOption.WOW_Test);
-    // chooser.addOption("Test1", TargetPoseOption.WOW_Test1);
-    chooser.addOption("Reef B", TargetPoseOption.REEF_B);
-    chooser.addOption("Reef E", TargetPoseOption.REEF_E);
-    chooser.addOption("Reef F", TargetPoseOption.REEF_F);
-    chooser.addOption("Reef H", TargetPoseOption.REEF_H);
-    chooser.addOption("Reef I", TargetPoseOption.REEF_I);
-    chooser.addOption("Reef J", TargetPoseOption.REEF_J);
-    chooser.addOption("Reef K", TargetPoseOption.REEF_K);
     SmartDashboard.putData("Pose choices", chooser);
 
     // Define behavior for chosing destination of on-the-fly pose
@@ -178,60 +132,12 @@ public static void setupController(
                     AutoBuilder.pathfindToPose(poseProcessor, constraints, 0.0)),
                 Map.entry(
                     TargetPoseOption.REEF_A.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0)),
-                // AutoBuilder.pathfindToPose(poseReefA, constraints, 0.0)
-                //     .andThen(AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0))),
+                    AutoBuilder.pathfindToPose(poseReefA, constraints, 0.5)
+                        .andThen(AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0))),
                 Map.entry(
                     TargetPoseOption.REEF_G.getIndex(),
                     AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
-                        .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
-                // AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.REEF_C.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefC, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.REEF_D.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefD, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.REEF_L.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefL, constraints, 0.0)),
-                // Map.entry(
-                //     TargetPoseOption.WOW_Test.getIndex(),
-                //     AutoBuilder.pathfindToPose(poseTest, constraints, 0.0)
-                //         .andThen(AutoBuilder.pathfindToPose(poseTest, constraints, 0.0))),
-                // Map.entry(
-                //     TargetPoseOption.WOW_Test1.getIndex(),
-                //     AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0)
-                //         .andThen(AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0))),
-                Map.entry(
-                    TargetPoseOption.REEF_B.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefB, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.REEF_E.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefE, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.REEF_F.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefF, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.REEF_H.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefHStart, constraints, 0.0)
-                        .andThen(
-                            new SequentialCommandGroup(
-                                new ElevatorToPosition(elevator, () -> 0.6),
-                                new ParallelCommandGroup(
-                                    new ArmToPosition(arm, () -> 75).withTimeout(1),
-                                    new ElevatorToPosition(elevator, () -> 1.553))))
-                        .andThen(AutoBuilder.pathfindToPose(poseReefHEnd, constraints, 0.0))
-                        .andThen(new ArmToPosition(arm, () -> 0))),
-                Map.entry(
-                    TargetPoseOption.REEF_I.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefI, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.REEF_J.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefJ, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.REEF_K.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefK, constraints, 0.0))),
+                        .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)))),
             () -> {
               return myCoolPoseKeyIdx;
             });
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index 2df7da0a..459c223f 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -37,13 +37,9 @@ public static class Constants {
   private SwerveDrive swerveDrive;
   @AutoLogOutput private boolean fieldOrientedDrive = false;
   private PIDConstants translationPIDConstants =
-      new PIDConstants(7.5, 0.0, 0.0); // Translation PID constants
-  // private PIDConstants rotationPIDConstants =
-  //     new PIDConstants(8.0, 0.0, 1.0); // Rotation PID constants
-  // private PIDConstants translationPIDConstants =
-  //     new PIDConstants(7.0, 0.0, 0.3); // Translation PID constants
+      new PIDConstants(5.0, 0.0, 0.0); // Translation PID constants
   private PIDConstants rotationPIDConstants =
-      new PIDConstants(6.0, 0.0, 0.0); // Rotation PID constants
+      new PIDConstants(5.0, 0.0, 0.0); // Rotation PID constants
 
   // @AutoLogOutput
   DriveIO io = new DriveIO();

From 3b136f7fb0519f7228712235630e3c1f6ed6811d Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Fri, 7 Mar 2025 18:25:23 -0500
Subject: [PATCH 36/90] temporarily unmap rear camera

---
 .../game/reefscape2025/RobotConfigComp.java   | 24 +++++++++----------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 89618292..63845330 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -42,18 +42,18 @@ public RobotConfigComp() {
     drive = new DriveSwerveYAGSL("yagsl/comp");
 
     // Cameras
-    vision.addCamera(
-        new Camera(
-            "rear_cam",
-            new Transform3d(
-                new Translation3d(
-                    Units.inchesToMeters(0.295),
-                    Units.inchesToMeters(-11.443),
-                    Units.inchesToMeters(39.663)),
-                new Rotation3d(
-                    Units.degreesToRadians(12),
-                    Units.degreesToRadians(-33),
-                    Units.degreesToRadians(170)))));
+    // vision.addCamera(
+    //     new Camera(
+    //         "rear_cam",
+    //         new Transform3d(
+    //             new Translation3d(
+    //                 Units.inchesToMeters(0.295),
+    //                 Units.inchesToMeters(-11.443),
+    //                 Units.inchesToMeters(39.663)),
+    //             new Rotation3d(
+    //                 Units.degreesToRadians(12),
+    //                 Units.degreesToRadians(-33),
+    //                 Units.degreesToRadians(170)))));
     vision.addCamera(
         new Camera(
             "left_cam", // left

From 2860493b1cb33d2a6db16ff2f2741e8aa7acbffd Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Fri, 7 Mar 2025 18:41:27 -0500
Subject: [PATCH 37/90] Logging flipped pose

---
 .../java/frc/robot/io/interfaces/DriveIO.java | 24 +++++++------------
 1 file changed, 9 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/io/interfaces/DriveIO.java b/src/main/java/frc/robot/io/interfaces/DriveIO.java
index 7eac4ea7..2a798041 100644
--- a/src/main/java/frc/robot/io/interfaces/DriveIO.java
+++ b/src/main/java/frc/robot/io/interfaces/DriveIO.java
@@ -1,5 +1,6 @@
 package frc.robot.io.interfaces;
 
+import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import org.littletonrobotics.junction.AutoLog;
 import swervelib.SwerveDrive;
@@ -11,7 +12,10 @@ public static class DriveIOInputs {
     public double poseX = 0.0;
     public double poseY = 0.0;
     public double poseRotInDegrees = 0.0;
-    public double distanceFromSpeaker = 0;
+    public Pose2d flippedPose;
+    public double flippedPoseX = 0.0;
+    public double flippedPoseY = 0.0;
+    public double flippedPoseRotInDegrees = 0.0;
   }
 
   /** Updates the set of loggable inputs. */
@@ -20,20 +24,10 @@ public void updateInputs(DriveIOInputs inputs, SwerveDrive swerveDrive) {
     inputs.poseX = inputs.pose.getTranslation().getX();
     inputs.poseY = inputs.pose.getTranslation().getY();
     inputs.poseRotInDegrees = inputs.pose.getRotation().getDegrees();
-    /*
-        if (!DevilBotState.isRedAlliance()) {
-          inputs.distanceFromSpeaker =
-              Math.sqrt(
-                  Math.pow(inputs.poseX - DriveBase.Constants.blueSpeakerX, 2)
-                      + Math.pow(inputs.poseY - DriveBase.Constants.speakerY, 2));
-        } else {
-          inputs.distanceFromSpeaker =
-              Math.sqrt(
-                  Math.pow(inputs.poseX - DriveBase.Constants.redSpeakerX, 2)
-                      + Math.pow(inputs.poseY - DriveBase.Constants.speakerY, 2));
-        }
-        ;
-    */
+    inputs.flippedPose = FlippingUtil.flipFieldPose(inputs.pose);
+    inputs.flippedPoseX = inputs.pose.getTranslation().getX();
+    inputs.flippedPoseY = inputs.pose.getTranslation().getY();
+    inputs.flippedPoseRotInDegrees = inputs.pose.getRotation().getDegrees();
   }
   // Other methods for controlling the drive subsystem...
 }

From c1691e7181681b038e6ea42cd7bb0a5ee1a6f63d Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Fri, 7 Mar 2025 21:04:24 -0500
Subject: [PATCH 38/90] lala

---
 .../game/reefscape2025/RobotConfig.java       |  1 +
 .../controls/drive/DriveControls.java         | 90 -------------------
 2 files changed, 1 insertion(+), 90 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index faeb0e1c..84ae11a2 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -211,6 +211,7 @@ public void configureBindings() {
     vision.setVisionMeasurementConsumer(drive::addVisionMeasurement);
 
     DriveControls.setupController(drive, elevator, coralArm, mainController);
+    DriveControls.setupAssistantController(drive, assistController);
     DriverAssistControls.setupController(elevator, coralArm, climberArm, assistController);
     DriverControls.setupController(elevator, coralArm, climberArm, mainController);
     PitControls.setupPitControls(elevator, coralArm, climberArm);
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index ed3d843d..e5007131 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -120,96 +120,6 @@ public static void setupController(Drive drive, Elevator elevator, Arm arm, Comm
     //               SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
     //             }));
 
-    // Define command to go to specific pose
-    // Command coolGoToPose =
-    //     new SelectCommand<>(
-    //         Map.ofEntries(
-    //             Map.entry(
-    //                 TargetPose.ORIGIN.getIndex(),
-    //                 AutoBuilder.pathfindToPose(poseOrigin, constraints, 0.0)),
-    //             Map.entry(
-    //                 TargetPose.FEEDER_1.getIndex(),
-    //                 AutoBuilder.pathfindToPose(poseFeeder1, constraints, 0.0)),
-    //             Map.entry(
-    //                 TargetPoseOption.FEEDER_2.getIndex(),
-    //                 AutoBuilder.pathfindToPose(poseFeeder2, constraints, 0.0)),
-    //             Map.entry(
-    //                 4, // processor
-    //                 AutoBuilder.pathfindToPose(poseProcessor, constraints, 0.0)),
-    //             coolDynamicPathScoringCommand(
-    //                 TargetPose.REEF_A,
-    //                 constraints,
-    //                 DriverControls.getPrepareToScoreCommand(elevator, arm),
-    //                 new ArmToPosition(arm, () -> 0)),
-    //             // AutoBuilder.pathfindToPose(poseReefA, constraints, 0.0)
-    //             //     .andThen(AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0))),
-    //             // Map.entry(
-    //             //     TargetPoseOption.REEF_G.getIndex(),
-    //             //     AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
-    //             //         .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0))),
-    //             coolDynamicPathScoringCommand(
-    //                 TargetPose.REEF_G,
-    //                 constraints,
-    //                 DriverControls.getPrepareToScoreCommand(elevator, arm),
-    //                 new ArmToPosition(arm, () -> 0)),
-    //             // AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)),
-    //             Map.entry(
-    //                 TargetPoseOption.REEF_C.getIndex(),
-    //                 AutoBuilder.pathfindToPose(poseReefC, constraints, 0.0)),
-    //             Map.entry(
-    //                 TargetPoseOption.REEF_D.getIndex(),
-    //                 AutoBuilder.pathfindToPose(poseReefD, constraints, 0.0)),
-    //             Map.entry(
-    //                 TargetPoseOption.REEF_L.getIndex(),
-    //                 AutoBuilder.pathfindToPose(poseReefL, constraints, 0.0)),
-    //             // Map.entry(
-    //             //     TargetPoseOption.WOW_Test.getIndex(),
-    //             //     AutoBuilder.pathfindToPose(poseTest, constraints, 0.0)
-    //             //         .andThen(AutoBuilder.pathfindToPose(poseTest, constraints, 0.0))),
-    //             // Map.entry(
-    //             //     TargetPoseOption.WOW_Test1.getIndex(),
-    //             //     AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0)
-    //             //         .andThen(AutoBuilder.pathfindToPose(poseTest2, constraints, 0.0))),
-    //             Map.entry(
-    //                 TargetPoseOption.REEF_B.getIndex(),
-    //                 AutoBuilder.pathfindToPose(poseReefB, constraints, 0.0)),
-    //             Map.entry(
-    //                 TargetPoseOption.REEF_E.getIndex(),
-    //                 AutoBuilder.pathfindToPose(poseReefE, constraints, 0.0)),
-    //             Map.entry(
-    //                 10, // need reee
-    //                 AutoBuilder.pathfindToPose(poseReefF, constraints, 0.0)),
-    //             createDynamicPathScoringCommand(
-    //                 TargetPoseOption.REEF_H.getIndex(),
-    //                 poseReefHStart,
-    //                 poseReefHEnd,
-    //                 constraints,
-    //                 DriverControls.getPrepareToScoreCommand(elevator, arm),
-    //                 new ArmToPosition(arm, () -> 0)),
-    //             // Map.entry(
-    //             //     TargetPoseOption.REEF_H.getIndex(),
-    //             //     AutoBuilder.pathfindToPose(poseReefHStart, constraints, 0.0)
-    //             //         .andThen(
-    //             //             new SequentialCommandGroup(
-    //             //                 new ElevatorToPosition(elevator, () -> 0.6),
-    //             //                 new ParallelCommandGroup(
-    //             //                     new ArmToPosition(arm, () -> 75).withTimeout(1),
-    //             //                     new ElevatorToPosition(elevator, () -> 1.553))))
-    //             //         .andThen(AutoBuilder.pathfindToPose(poseReefHEnd, constraints, 0.0))
-    //             //         .andThen(new ArmToPosition(arm, () -> 0))),
-    //             Map.entry(
-    //                 TargetPoseOption.REEF_I.getIndex(),
-    //                 AutoBuilder.pathfindToPose(poseReefI, constraints, 0.0)),
-    //             Map.entry(
-    //                 TargetPoseOption.REEF_J.getIndex(),
-    //                 AutoBuilder.pathfindToPose(poseReefJ, constraints, 0.0)),
-    //             Map.entry(
-    //                 TargetPoseOption.REEF_K.getIndex(),
-    //                 AutoBuilder.pathfindToPose(poseReefK, constraints, 0.0))),
-    //         () -> {
-    //           return myCoolPoseKeyIdx;
-    //         });
-
     // Define command to go to specific pose
     Command coolGoToPose =
         new SelectCommand<>(

From 60d7066d3bda58750b091f6a0d6ba9c84eea83de Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Sat, 8 Mar 2025 00:43:21 -0500
Subject: [PATCH 39/90] default field oriented. Fix AutoBuilder PID

---
 .../subsystems/implementations/drive/DriveSwerveYAGSL.java    | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index 9005516b..c5f58671 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -35,11 +35,11 @@ public static class Constants {
 
   private final File swerveJsonDirectory;
   private SwerveDrive swerveDrive;
-  @AutoLogOutput private boolean fieldOrientedDrive = false;
+  @AutoLogOutput private boolean fieldOrientedDrive = true;
   private PIDConstants translationPIDConstants =
       new PIDConstants(7.5, 0.0, 0.0); // Translation PID constants
   private PIDConstants rotationPIDConstants =
-      new PIDConstants(5.0, 0.0, 0.0); // Rotation PID constants
+      new PIDConstants(6.0, 0.0, 0.0); // Rotation PID constants
 
   // @AutoLogOutput
   DriveIO io = new DriveIO();

From 2ffa7aa6e3384ccd274842e5b7811401c93d3ffb Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Sat, 8 Mar 2025 01:05:31 -0500
Subject: [PATCH 40/90] hooking in Autonomous routines

---
 .../game/reefscape2025/RobotConfig.java       |  4 +
 .../game/reefscape2025/RobotConfigComp.java   |  4 +
 .../controls/drive/DriveControls.java         | 77 +++++++++++++++----
 3 files changed, 70 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index 84ae11a2..3bb0661f 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -1,5 +1,6 @@
 package frc.robot.config.game.reefscape2025;
 
+import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.controller.ArmFeedforward;
@@ -205,6 +206,9 @@ public RobotConfig(
   public void configureBindings() {
     if (Robot.isSimulation()) {
       vision.enableSimulation(() -> RobotConfig.drive.getPose(), true);
+
+      // HACK just to verify autos are visible without connecting to robot
+      RobotConfig.autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
     }
 
     // Send vision-based odometry measurements to drive's odometry calculations
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index a5531f59..a3d83be5 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -1,5 +1,6 @@
 package frc.robot.config.game.reefscape2025;
 
+import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
@@ -216,5 +217,8 @@ public RobotConfigComp() {
       led.setData(ledBuffer);
       led.start();
     }
+
+    // Auto(s)
+    autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index e5007131..92d8cc53 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -20,7 +20,6 @@
 import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Drive;
 import frc.robot.subsystems.interfaces.Elevator;
-
 import java.util.Map;
 import java.util.Map.Entry;
 
@@ -34,7 +33,8 @@ public class DriveControls {
     "E", "F", "G", "H", "I", "J", "K", "L", "A", "B", "C", "D"
   };
 
-  public static void setupController(Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
+  public static void setupController(
+      Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
     SubsystemBase driveSubsystem = (SubsystemBase) drive;
     driveSubsystem.setDefaultCommand(
         new DriveCommand(
@@ -124,19 +124,66 @@ public static void setupController(Drive drive, Elevator elevator, Arm arm, Comm
     Command coolGoToPose =
         new SelectCommand<>(
             Map.ofEntries(
-                coolDynamicPathScoringCommand(TargetPose.REEF_A, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_B, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_C, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_D, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_E, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_F, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_G, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_H, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_I, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_J, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_K, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0)),
-                coolDynamicPathScoringCommand(TargetPose.REEF_L, constraints, DriverControls.getPrepareToScoreCommand(elevator, arm), new ArmToPosition(arm, () -> 0))
-            ),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_A,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_B,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_C,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_D,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_E,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_F,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_G,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_H,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_I,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_J,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_K,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_L,
+                    constraints,
+                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0))),
             () -> {
               return myCoolPoseKeyIdx;
             });

From 556af450f9df4cca257b708812e3a244740f35c2 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Sat, 8 Mar 2025 01:13:08 -0500
Subject: [PATCH 41/90] Hook up webcam to network tables on comp bot

---
 .../frc/robot/config/game/reefscape2025/RobotConfigComp.java  | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index a3d83be5..a3325125 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -1,6 +1,7 @@
 package frc.robot.config.game.reefscape2025;
 
 import com.pathplanner.lib.auto.AutoBuilder;
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
@@ -220,5 +221,8 @@ public RobotConfigComp() {
 
     // Auto(s)
     autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
+
+    // Start webcam
+    CameraServer.startAutomaticCapture();
   }
 }

From d068499f958d20c4b4d32e7d017abf7988a023a0 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Sat, 8 Mar 2025 01:18:49 -0500
Subject: [PATCH 42/90] return of the climber

---
 .../game/reefscape2025/RobotConfigComp.java   | 73 ++++++++++---------
 1 file changed, 38 insertions(+), 35 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index a3325125..a0b955f2 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -5,6 +5,7 @@
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -18,15 +19,18 @@
 import frc.robot.io.implementations.motor.MotorIOBase.MotorIOBaseSettings;
 import frc.robot.io.implementations.motor.MotorIOTalonFx;
 import frc.robot.io.implementations.motor.MotorIOTalonFx.TalonFxSettings;
+import frc.robot.subsystems.controls.arm.ClimberArmControls;
 import frc.robot.subsystems.controls.arm.CoralArmControls;
 import frc.robot.subsystems.controls.elevator.ElevatorControls;
 import frc.robot.subsystems.implementations.drive.DriveBase;
 import frc.robot.subsystems.implementations.drive.DriveSwerveYAGSL;
 import frc.robot.subsystems.implementations.motor.ArmMotorSubsystem;
 import frc.robot.subsystems.implementations.motor.ElevatorMotorSubsystem;
+import frc.robot.subsystems.implementations.motor.SimpleMotorSubsystem;
 import frc.robot.subsystems.interfaces.Arm.ArmSettings;
 import frc.robot.subsystems.interfaces.Drive;
 import frc.robot.subsystems.interfaces.Elevator.ElevatorSettings;
+import frc.robot.subsystems.interfaces.SimpleMotor.SimpleMotorSettings;
 import frc.robot.subsystems.interfaces.Vision.Camera;
 
 public class RobotConfigComp extends RobotConfig {
@@ -34,7 +38,7 @@ public class RobotConfigComp extends RobotConfig {
   private final AddressableLEDBuffer ledBuffer;
 
   public RobotConfigComp() {
-    super(false, false, false, false, false, true, true);
+    super(false, false, false, false, false, true, false);
 
     // Comp has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
@@ -169,40 +173,39 @@ public RobotConfigComp() {
               new MotorIOTalonFx(motorSettings, talonFxSettings), "Coral", armSettings);
     }
 
-    // // climber
-    // {
-    //   MotorIOBaseSettings motorSettings = new MotorIOBaseSettings();
-    //   // 25:1 gear box ratio
-    //   motorSettings.motor.gearing = 25;
-    //   motorSettings.motor.inverted = false;
-    //   motorSettings.pid = new PIDController(1.0, 0, 0);
-    //   motorSettings.reverseLimitChannel = 1;
-    //   motorSettings.reverseLimitNegate = true;
-
-    //   SimpleMotorSettings simpleMotorSettings = new SimpleMotorSettings();
-    //   simpleMotorSettings.minPositionInRads = 0;
-    //   simpleMotorSettings.maxPositionInRads = 36.1;
-    //   simpleMotorSettings.startingPositionInRads = simpleMotorSettings.minPositionInRads;
-    //   simpleMotorSettings.color = new Color8Bit(Color.kRed);
-    //   simpleMotorSettings.feedforward = new SimpleMotorFeedforward(0, 0, 0);
-    //   simpleMotorSettings.motor = DCMotor.getNEO(1);
-
-    //   TalonFxSettings settings = new TalonFxSettings();
-    //   settings.canId = 50;
-
-    //   ClimberArmControls.Constants.autoZeroSettings.voltage = -1;
-    //   // Set this to something big, we are never going to use stall current to detect if climber
-    // has
-    //   // reached it's end of range of motion.
-    //   ClimberArmControls.Constants.autoZeroSettings.minResetCurrent = 0.5;
-    //   ClimberArmControls.Constants.autoZeroSettings.resetPositionRad =
-    //       simpleMotorSettings.minPositionInRads;
-    //   ClimberArmControls.Constants.autoZeroSettings.initialReverseDuration = 0;
-
-    //   climberArm =
-    //       new SimpleMotorSubsystem(
-    //           new MotorIOTalonFx(motorSettings, settings), "Climber", simpleMotorSettings);
-    // }
+    // climber
+    {
+      MotorIOBaseSettings motorSettings = new MotorIOBaseSettings();
+      // 25:1 gear box ratio
+      motorSettings.motor.gearing = 25;
+      motorSettings.motor.inverted = false;
+      motorSettings.pid = new PIDController(1.0, 0, 0);
+      motorSettings.reverseLimitChannel = 1;
+      motorSettings.reverseLimitNegate = true;
+
+      SimpleMotorSettings simpleMotorSettings = new SimpleMotorSettings();
+      simpleMotorSettings.minPositionInRads = 0;
+      simpleMotorSettings.maxPositionInRads = 36.1;
+      simpleMotorSettings.startingPositionInRads = simpleMotorSettings.minPositionInRads;
+      simpleMotorSettings.color = new Color8Bit(Color.kRed);
+      simpleMotorSettings.feedforward = new SimpleMotorFeedforward(0, 0, 0);
+      simpleMotorSettings.motor = DCMotor.getNEO(1);
+
+      TalonFxSettings settings = new TalonFxSettings();
+      settings.canId = 50;
+
+      ClimberArmControls.Constants.autoZeroSettings.voltage = -1;
+      // Set this to something big, we are never going to use stall current to detect if climber has
+      // reached it's end of range of motion.
+      ClimberArmControls.Constants.autoZeroSettings.minResetCurrent = 0.5;
+      ClimberArmControls.Constants.autoZeroSettings.resetPositionRad =
+          simpleMotorSettings.minPositionInRads;
+      ClimberArmControls.Constants.autoZeroSettings.initialReverseDuration = 0;
+
+      climberArm =
+          new SimpleMotorSubsystem(
+              new MotorIOTalonFx(motorSettings, settings), "Climber", simpleMotorSettings);
+    }
 
     // LED
     {

From 98c2cf2f185ee144f6d832b95baba754381e06d2 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Sat, 8 Mar 2025 11:21:32 -0500
Subject: [PATCH 43/90] re add named commands

---
 .../game/reefscape2025/RobotConfigComp.java    | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index a0b955f2..6566e559 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -1,6 +1,8 @@
 package frc.robot.config.game.reefscape2025;
 
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
@@ -16,6 +18,8 @@
 import edu.wpi.first.wpilibj.LEDPattern;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
+import frc.robot.commands.common.arm.ArmToPosition;
+import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.io.implementations.motor.MotorIOBase.MotorIOBaseSettings;
 import frc.robot.io.implementations.motor.MotorIOTalonFx;
 import frc.robot.io.implementations.motor.MotorIOTalonFx.TalonFxSettings;
@@ -223,6 +227,20 @@ public RobotConfigComp() {
     }
 
     // Auto(s)
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.5 meter", new ElevatorToPosition(elevator, () -> 0.5));
+    NamedCommands.registerCommand(
+        "Move Elevator to 1.553 meter", new ElevatorToPosition(elevator, () -> 1.553));
+    NamedCommands.registerCommand(
+        "Move Arm 75 degrees", new ArmToPosition(coralArm, () -> 70).withTimeout(1.5));
+    NamedCommands.registerCommand(
+        "Move Arm 0 degrees", new ArmToPosition(coralArm, () -> 0).withTimeout(1.5));
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.8 meter", new ElevatorToPosition(elevator, () -> 0.8));
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.4 meter", new ElevatorToPosition(elevator, () -> 0.4));
+    NamedCommands.registerCommand(
+        "Move Arm for Intake", new ArmToPosition(coralArm, () -> -90).withTimeout(0));
     autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
 
     // Start webcam

From e5d063e3d592c890d5d144cf47ac5ad8503eb32a Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Sat, 8 Mar 2025 12:48:41 -0500
Subject: [PATCH 44/90] inserted poses taken during field practice field
 testing

---
 .../subsystems/controls/drive/TargetPose.java | 56 +++++++++++++++----
 1 file changed, 45 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 54c61978..d47caa5e 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -12,20 +12,54 @@ public enum TargetPose {
   ORIGIN(0, "O", "Origin", new Pose2d(0.0, 0.0, new Rotation2d(0)), 0),
   FEEDER_R(1, "FR", "Feeder Right", new Pose2d(1.05, 1, new Rotation2d(50)), 0),
   FEEDER_L(2, "FL", "Feeder Left", new Pose2d(1.05, 7, Rotation2d.fromDegrees(-50)), 0),
-  REEF_A(3, "A", "Reef A", new Pose2d(3.15, 4.28, Rotation2d.fromDegrees(0)), -1),
-  REEF_B(4, "B", "Reef B", new Pose2d(3.15, 3.70, Rotation2d.fromDegrees(0)), 1),
-  REEF_C(5, "C", "Reef C", new Pose2d(3.60, 2.93, Rotation2d.fromDegrees(60)), -1),
-  REEF_D(6, "D", "Reef D", new Pose2d(3.96, 2.74, Rotation2d.fromDegrees(60)), 1),
-  REEF_E(7, "E", "Reef E", new Pose2d(5.10, 2.75, Rotation2d.fromDegrees(124)), -1),
-  REEF_F(8, "F", "Reef F", new Pose2d(5.36, 2.93, Rotation2d.fromDegrees(124)), 1),
-  REEF_G(9, "G", "Reef G", new Pose2d(5.5, 3.95, Rotation2d.fromDegrees(180)), -1),
+  REEF_A(3, "A", "Reef A", new Pose2d(3.14, 4.18, Rotation2d.fromDegrees(0)), -1),
+  REEF_B(4, "B", "Reef B", new Pose2d(3.14, 3.79, Rotation2d.fromDegrees(0)), 1),
+  REEF_C(5, "C", "Reef C", new Pose2d(3.71, 2.98, Rotation2d.fromDegrees(60)), -1),
+  REEF_D(6, "D", "Reef D", new Pose2d(4.10, 2.76, Rotation2d.fromDegrees(60)), 1),
+  REEF_E(7, "E", "Reef E", new Pose2d(5.00, 2.83, Rotation2d.fromDegrees(120)), -1),
+  REEF_F(8, "F", "Reef F", new Pose2d(5.36, 3.00, Rotation2d.fromDegrees(122)), 1),
+  REEF_G(9, "G", "Reef G", new Pose2d(5.71, 4.00, Rotation2d.fromDegrees(183)), -1),
   REEF_H(10, "H", "Reef H", new Pose2d(5.80, 4.20, Rotation2d.fromDegrees(180)), 1),
-  REEF_I(11, "I", "Reef I", new Pose2d(5.30, 5.15, Rotation2d.fromDegrees(-124)), -1),
-  REEF_J(12, "J", "Reef J", new Pose2d(5.00, 5.30, Rotation2d.fromDegrees(-124)), 1),
-  REEF_K(13, "K", "Reef K", new Pose2d(4.00, 5.33, Rotation2d.fromDegrees(-60)), -1),
-  REEF_L(14, "L", "Reef L", new Pose2d(3.65, 5.18, Rotation2d.fromDegrees(-60)), 1),
+  REEF_I(11, "I", "Reef I", new Pose2d(5.28, 5.06, Rotation2d.fromDegrees(-120)), -1),
+  REEF_J(12, "J", "Reef J", new Pose2d(4.97, 5.28, Rotation2d.fromDegrees(-120)), 1),
+  REEF_K(13, "K", "Reef K", new Pose2d(3.95, 5.18, Rotation2d.fromDegrees(-60)), -1),
+  REEF_L(14, "L", "Reef L", new Pose2d(3.69, 5.04, Rotation2d.fromDegrees(-60)), 1),
   PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0);
 
+  
+
+  /*
+   *  Practice field testing
+   * A: 3.13 4.18 0           14.36 3.88 180
+   * B: 3.13 3.79 0           14.42 4.26 180
+   * C: 3.71 2.98 59.6        13.83 5.07 -120.3
+   * D: 4.10 2.76 60          13.45 5.29 -120
+   * E: 5.00 2.83 120.0       12.55 5.23 -60
+   * F: 5.36 3.00 122.5~      12.19 5.06 -57.7    ** this one potentially uncentered
+   * G: 5.71 4.00 -177.23     11.84 4.07 2.77
+   * H: 
+   * I: 5.28 5.06 -119.74     12.26 3.00 60.26
+   * J: 4.97 5.28 -119.62     12.57 2.78 60.38
+   * K: 3.95 5.18 -59         13.59 2.87 121
+   * L: 3.69 5.04 -58.6~      13.86 3.01 121.3         
+   */
+
+  /*
+   *  During practice matches
+   * A:
+   * B: 
+   * C: 
+   * D: 
+   * E:
+   * F:
+   * G: 5.55 3.83 178.7       11.99 4.22 -1.30
+   * H: 5.56 4.20 177.95      11.99 3.85 -3.05
+   * I: 5.27 5.03 -125.37     12.28 3.02 54.63    ?
+   * J: 4.71 5.14 -123.69     12.84 2.92 56.31    ??
+   * K:
+   * L? 3.49 4.61 -57.59      14.06 3.44 122.41   ??
+   */
+
   private int index;
   private String shortName;
   private String longName;

From d43e8f33f85748f65435b221341ebe33033347e8 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Sat, 8 Mar 2025 13:12:31 -0500
Subject: [PATCH 45/90] lower intake

---
 .../robot/subsystems/controls/combination/DriverControls.java   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 5448a4a7..771f92dc 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -53,7 +53,7 @@ public static void setupController(
                           DriverControls.Constants.prepareScoreSelctedIndex);
                     })));
 
-    Command intakeCoralCommand = new ElevatorToPosition(elevator, () -> 0.35);
+    Command intakeCoralCommand = new ElevatorToPosition(elevator, () -> 0.3                                                                                                                                                                                                 );
     Trigger scoreMode = new Trigger(() -> Constants.prepareScoreSelctedIndex >= 2);
     controller.leftTrigger().and(scoreMode.negate()).and(ableToIntake).onTrue(intakeCoralCommand);
 

From 20a0dccc4e8675480226e55ec0f38d942cae4f2c Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Sat, 8 Mar 2025 14:10:42 -0500
Subject: [PATCH 46/90] Made F4-X-C4 Auto. Needs to be tested

---
 .../autos/Score F L4 + Y + C L4 .auto         | 176 ++++++++++++++++++
 .../deploy/pathplanner/autos/Score F L4.auto  |  82 ++++++++
 src/main/deploy/pathplanner/paths/Back C.path |  54 ++++++
 src/main/deploy/pathplanner/paths/Back F.path |  54 ++++++
 .../pathplanner/paths/F to Source X.path      |  54 ++++++
 .../deploy/pathplanner/paths/Get to F.path    |  54 ++++++
 .../pathplanner/paths/Source X to C.path      |  54 ++++++
 .../pathplanner/paths/Start to Near F.path    |  54 ++++++
 .../game/reefscape2025/RobotConfigComp.java   |   1 -
 .../controls/combination/DriverControls.java  |   2 +-
 .../subsystems/controls/drive/TargetPose.java |  12 +-
 11 files changed, 588 insertions(+), 9 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto
 create mode 100644 src/main/deploy/pathplanner/autos/Score F L4.auto
 create mode 100644 src/main/deploy/pathplanner/paths/Back C.path
 create mode 100644 src/main/deploy/pathplanner/paths/Back F.path
 create mode 100644 src/main/deploy/pathplanner/paths/F to Source X.path
 create mode 100644 src/main/deploy/pathplanner/paths/Get to F.path
 create mode 100644 src/main/deploy/pathplanner/paths/Source X to C.path
 create mode 100644 src/main/deploy/pathplanner/paths/Start to Near F.path

diff --git a/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto
new file mode 100644
index 00000000..bc8c86d9
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto	
@@ -0,0 +1,176 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near F"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to F"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back F"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "F to Source X"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Elevator to 0.8 meter"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Arm for Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 4.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Elevator to 0.4 meter"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Source X to C"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.4 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back C"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score F L4.auto b/src/main/deploy/pathplanner/autos/Score F L4.auto
new file mode 100644
index 00000000..ab2a2366
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score F L4.auto	
@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near F"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to F"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back F"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back C.path b/src/main/deploy/pathplanner/paths/Back C.path
new file mode 100644
index 00000000..e654c08d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Back C.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6595738636363637,
+        "y": 2.943082386363636
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6156440319325927,
+        "y": 2.6969723095961897
+      },
+      "isLocked": false,
+      "linkedName": "C Reef"
+    },
+    {
+      "anchor": {
+        "x": 3.5199715909090905,
+        "y": 2.6937926136363637
+      },
+      "prevControl": {
+        "x": 3.528294263800622,
+        "y": 2.9436540414637506
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back F.path b/src/main/deploy/pathplanner/paths/Back F.path
new file mode 100644
index 00000000..c3750a7e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Back F.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.265,
+        "y": 2.933110795454545
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.544368285561805,
+        "y": 2.804956864154667
+      },
+      "isLocked": false,
+      "linkedName": "F Reef"
+    },
+    {
+      "anchor": {
+        "x": 5.394630681818183,
+        "y": 2.77356534090909
+      },
+      "prevControl": {
+        "x": 5.019288168232361,
+        "y": 2.8766904771262314
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near F"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 122.66091272167375
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 122.66
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/F to Source X.path b/src/main/deploy/pathplanner/paths/F to Source X.path
new file mode 100644
index 00000000..7afb4b08
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/F to Source X.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.394630681818183,
+        "y": 2.77356534090909
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.676676136363636,
+        "y": 2.2251278409090913
+      },
+      "isLocked": false,
+      "linkedName": "Near F"
+    },
+    {
+      "anchor": {
+        "x": 1.1766477272727274,
+        "y": 0.7393607954545461
+      },
+      "prevControl": {
+        "x": 2.3931818181818194,
+        "y": 1.0285369318181825
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Source X"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 53.84
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 122.66091272167375
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to F.path b/src/main/deploy/pathplanner/paths/Get to F.path
new file mode 100644
index 00000000..efcc9d7e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Get to F.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.394630681818183,
+        "y": 2.77356534090909
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.218864011662972,
+        "y": 2.953868223607026
+      },
+      "isLocked": false,
+      "linkedName": "Near F"
+    },
+    {
+      "anchor": {
+        "x": 5.265,
+        "y": 2.933110795454545
+      },
+      "prevControl": {
+        "x": 5.453247064609036,
+        "y": 2.7490837110619077
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "F Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 122.66
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 122.66091272167375
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Source X to C.path b/src/main/deploy/pathplanner/paths/Source X to C.path
new file mode 100644
index 00000000..ef965bb1
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Source X to C.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1766477272727274,
+        "y": 0.7393607954545461
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.389856826038373,
+        "y": 0.869905348016272
+      },
+      "isLocked": false,
+      "linkedName": "Source X"
+    },
+    {
+      "anchor": {
+        "x": 3.6595738636363637,
+        "y": 2.943082386363636
+      },
+      "prevControl": {
+        "x": 3.3196597852791605,
+        "y": 2.661497390465073
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "C Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 53.84
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to Near F.path b/src/main/deploy/pathplanner/paths/Start to Near F.path
new file mode 100644
index 00000000..afba264d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Start to Near F.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.857613636363637,
+        "y": 1.9259801136363626
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.89811462363063,
+        "y": 2.117009249713284
+      },
+      "isLocked": false,
+      "linkedName": "Left Start"
+    },
+    {
+      "anchor": {
+        "x": 5.394630681818183,
+        "y": 2.77356534090909
+      },
+      "prevControl": {
+        "x": 5.874725597416754,
+        "y": 2.6152866739994796
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near F"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 122.66091272167375
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 6566e559..173c2774 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -2,7 +2,6 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 771f92dc..d9a68ac6 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -53,7 +53,7 @@ public static void setupController(
                           DriverControls.Constants.prepareScoreSelctedIndex);
                     })));
 
-    Command intakeCoralCommand = new ElevatorToPosition(elevator, () -> 0.3                                                                                                                                                                                                 );
+    Command intakeCoralCommand = new ElevatorToPosition(elevator, () -> 0.3);
     Trigger scoreMode = new Trigger(() -> Constants.prepareScoreSelctedIndex >= 2);
     controller.leftTrigger().and(scoreMode.negate()).and(ableToIntake).onTrue(intakeCoralCommand);
 
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index d47caa5e..2143dc58 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -26,8 +26,6 @@ public enum TargetPose {
   REEF_L(14, "L", "Reef L", new Pose2d(3.69, 5.04, Rotation2d.fromDegrees(-60)), 1),
   PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0);
 
-  
-
   /*
    *  Practice field testing
    * A: 3.13 4.18 0           14.36 3.88 180
@@ -37,19 +35,19 @@ public enum TargetPose {
    * E: 5.00 2.83 120.0       12.55 5.23 -60
    * F: 5.36 3.00 122.5~      12.19 5.06 -57.7    ** this one potentially uncentered
    * G: 5.71 4.00 -177.23     11.84 4.07 2.77
-   * H: 
+   * H:
    * I: 5.28 5.06 -119.74     12.26 3.00 60.26
    * J: 4.97 5.28 -119.62     12.57 2.78 60.38
    * K: 3.95 5.18 -59         13.59 2.87 121
-   * L: 3.69 5.04 -58.6~      13.86 3.01 121.3         
+   * L: 3.69 5.04 -58.6~      13.86 3.01 121.3
    */
 
   /*
    *  During practice matches
    * A:
-   * B: 
-   * C: 
-   * D: 
+   * B:
+   * C:
+   * D:
    * E:
    * F:
    * G: 5.55 3.83 178.7       11.99 4.22 -1.30

From e9c10984eb17083aaa7360a1a37b7490d2e260d6 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Sat, 8 Mar 2025 14:38:16 -0500
Subject: [PATCH 47/90]  otf elevator arm

---
 .../controls/drive/DriveControls.java         | 215 ++++++++++--------
 1 file changed, 123 insertions(+), 92 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 92d8cc53..44f1c491 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -16,6 +17,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.drive.DriveCommand;
+import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.subsystems.controls.combination.DriverControls;
 import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Drive;
@@ -127,142 +129,171 @@ public static void setupController(
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_A,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0)),
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_B,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0)),
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_C,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0)),
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_D,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0)),
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_E,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0)),
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_F,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0)),
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_G,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0)),
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_H,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0)),
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_I,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0)),
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_J,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0)),
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_K,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0)),
                 coolDynamicPathScoringCommand(
                     TargetPose.REEF_L,
                     constraints,
-                    DriverControls.getPrepareToScoreCommand(elevator, arm),
+                    getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0))),
-            () -> {
-              return myCoolPoseKeyIdx;
-            });
-
-    // TODO: new dynamic path planning command creation
-    // 1. SelectCommand ONE: go to initial location (based on user-chosen option X)
-    // 2. SelectCommand TWO: determine next action (based on option X and potentially pre-decided
-    // reef elevation value)
-    // 2a. if (1) was a reef position, assume we are scoring --> get reef elevation to score on -->
-    // set elevator/arm
-    // 2b. if not, choose an empty command and short-circuit (exit) the sequence
-    // 3. SelectCommand THREE: go to scoring position (which maps directly from initial chosen
-    // location)
-    // 4. Execute <arm down command>
-
-    // dynamically go to destination
-    controller.rightTrigger().whileTrue(coolGoToPose);
-
-    // TEMP FUNCTION TO TEST FIELD FLIPPING
-    // controller.b().whileTrue(new SequentialCommandGroup(
-    //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPrepPose(), constraints, 0.0),
-    //     // DriverControls.Constants.prepareScoreCommand,
-    //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPose(), constraints, 0.0),
-    //     new ArmToPosition(arm, () -> 0)));
-
-    /*  Angles -> Reef positions
-     * 0    30  : E
-     * 30   60  : F
-     * 60   90  : G
-     * 90   120 : H
-     * 120  150 : I
-     * 150  180 : J
-     * 180  210 : K
-     * 210  240 : L
-     * 240  270 : A
-     * 270  300 : B
-     * 300  330 : C
-     * 330  360 : D
-     */
-  }
-
-  public static void setupAssistantController(Drive drive, CommandXboxController controller) {
-    controller
-        .x()
-        .whileTrue(
-            new RunCommand(
-                () -> {
-                  double myX = controller.getLeftX();
-                  double myY = -controller.getLeftY();
-
-                  // TODO maybe keep calculation in radians?
-                  double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
-                  if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
-
-                  // This number can be used to index into the ordered reef positions array!
-                  coolestNumberEver = (int) myNumber / 30;
-
-                  // TODO remove Smartdashboard number; only display reef position
-                  SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
-                  SmartDashboard.putString(
-                      "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
-                }));
-  }
-
-  private static Entry<Integer, Command> coolDynamicPathScoringCommand(
-      TargetPose target,
-      PathConstraints constraints,
-      Command prepareToScoreCommand,
-      Command scoreCommand) {
-    // FIXME initialize a fresh instance of the prepareToScoreCommand each time!!
-    // OTHERWISE CODE WILL CRASH
-    Entry<Integer, Command> entry =
-        Map.entry(
-            target.getIndex(),
-            new SequentialCommandGroup(
-                AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
-                prepareToScoreCommand,
-                AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
-                scoreCommand));
-    return entry;
+                                () -> {
+                                  return myCoolPoseKeyIdx;
+                                });
+                    
+                        // TODO: new dynamic path planning command creation
+                        // 1. SelectCommand ONE: go to initial location (based on user-chosen option X)
+                        // 2. SelectCommand TWO: determine next action (based on option X and potentially pre-decided
+                        // reef elevation value)
+                        // 2a. if (1) was a reef position, assume we are scoring --> get reef elevation to score on -->
+                        // set elevator/arm
+                        // 2b. if not, choose an empty command and short-circuit (exit) the sequence
+                        // 3. SelectCommand THREE: go to scoring position (which maps directly from initial chosen
+                        // location)
+                        // 4. Execute <arm down command>
+                    
+                        // dynamically go to destination
+                        controller.rightTrigger().whileTrue(coolGoToPose);
+                    
+                        // TEMP FUNCTION TO TEST FIELD FLIPPING
+                        // controller.b().whileTrue(new SequentialCommandGroup(
+                        //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPrepPose(), constraints, 0.0),
+                        //     // DriverControls.Constants.prepareScoreCommand,
+                        //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPose(), constraints, 0.0),
+                        //     new ArmToPosition(arm, () -> 0)));
+                    
+                        /*  Angles -> Reef positions
+                         * 0    30  : E
+                         * 30   60  : F
+                         * 60   90  : G
+                         * 90   120 : H
+                         * 120  150 : I
+                         * 150  180 : J
+                         * 180  210 : K
+                         * 210  240 : L
+                         * 240  270 : A
+                         * 270  300 : B
+                         * 300  330 : C
+                         * 330  360 : D
+                         */
+                      }
+                    
+                      public static void setupAssistantController(Drive drive, CommandXboxController controller) {
+                        controller
+                            .x()
+                            .whileTrue(
+                                new RunCommand(
+                                    () -> {
+                                      double myX = controller.getLeftX();
+                                      double myY = -controller.getLeftY();
+                    
+                                      // TODO maybe keep calculation in radians?
+                                      double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
+                                      if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
+                    
+                                      // This number can be used to index into the ordered reef positions array!
+                                      coolestNumberEver = (int) myNumber / 30;
+                    
+                                      // TODO remove Smartdashboard number; only display reef position
+                                      SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
+                                      SmartDashboard.putString(
+                                          "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
+                                    }));
+                      }
+                    
+                      private static Entry<Integer, Command> coolDynamicPathScoringCommand(
+                          TargetPose target,
+                          PathConstraints constraints,
+                          Command prepareToScoreCommand,
+                          Command scoreCommand) {
+                        // FIXME initialize a fresh instance of the prepareToScoreCommand each time!!
+                        // OTHERWISE CODE WILL CRASH
+                        Entry<Integer, Command> entry =
+                            Map.entry(
+                                target.getIndex(),
+                                new SequentialCommandGroup(
+                                    AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
+                                    prepareToScoreCommand,
+                                    AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
+                                    scoreCommand));
+                        return entry;
+                      }
+                    
+    private static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm) {
+    return new SelectCommand<>(
+        Map.ofEntries(
+            Map.entry(1, new InstantCommand(() -> System.out.println("Not scoring mode selected"))),
+            Map.entry(
+                2,
+                new SequentialCommandGroup(
+                        new ElevatorToPosition(elevator, () -> 0.63),
+                        new ArmToPosition(coralArm, () -> 47))
+                    .withTimeout(5.0)),
+            Map.entry(
+                3,
+                new SequentialCommandGroup(
+                        new ElevatorToPosition(elevator, () -> 1.0),
+                        new ArmToPosition(coralArm, () -> 47))
+                    .withTimeout(5.0)),
+            Map.entry(
+                4,
+                new SequentialCommandGroup(
+                        new ElevatorToPosition(elevator, () -> 0.6),
+                        new ParallelCommandGroup(
+                            new ArmToPosition(coralArm, () -> 48).withTimeout(1.0),
+                            new ElevatorToPosition(elevator, () -> 1.553)))
+                    .withTimeout(5.0))),
+        () -> {
+          return DriverControls.Constants.prepareScoreSelctedIndex;
+        });
   }
 }

From 7856d68b85a965a28597a8f44f894ecccd416123 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Sat, 8 Mar 2025 15:26:51 -0500
Subject: [PATCH 48/90] Changed G Auto.

---
 src/main/deploy/pathplanner/paths/Back G.path                 | 4 ++--
 src/main/deploy/pathplanner/paths/Get to G.path               | 4 ++--
 .../frc/robot/config/game/reefscape2025/RobotConfigComp.java  | 2 +-
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/Back G.path b/src/main/deploy/pathplanner/paths/Back G.path
index 36af730b..5ef72008 100644
--- a/src/main/deploy/pathplanner/paths/Back G.path	
+++ b/src/main/deploy/pathplanner/paths/Back G.path	
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.87,
+        "x": 5.723693181818183,
         "y": 3.85
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.141921436275931,
+        "x": 5.9956146180941134,
         "y": 3.840199234078392
       },
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Get to G.path b/src/main/deploy/pathplanner/paths/Get to G.path
index 19024cc8..6e04fcae 100644
--- a/src/main/deploy/pathplanner/paths/Get to G.path	
+++ b/src/main/deploy/pathplanner/paths/Get to G.path	
@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 5.87,
+        "x": 5.723693181818183,
         "y": 3.85
       },
       "prevControl": {
-        "x": 6.1192151813798015,
+        "x": 5.972908363197984,
         "y": 3.8697937709856585
       },
       "nextControl": null,
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 173c2774..5ee9f999 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -166,7 +166,7 @@ public RobotConfigComp() {
       CoralArmControls.Constants.autoZeroSettings.minResetCurrent = 0.5;
       CoralArmControls.Constants.autoZeroSettings.resetPositionRad =
           Units.degreesToRadians(
-              armSettings.minAngleInDegrees - 10); // We have an offest about 15 degrees
+              armSettings.minAngleInDegrees); // We have an offest about 15 degrees
       CoralArmControls.Constants.autoZeroSettings.initialReverseDuration =
           0.0; // Set the seconds of reverse before zero. Set to zero if there shound be no reverse
 

From 626dce3ead9d8299306e58fa3f7f1c1ee70afb91 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Sat, 8 Mar 2025 15:32:28 -0500
Subject: [PATCH 49/90] dial index

---
 .../subsystems/controls/drive/DriveControls.java     | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 44f1c491..ec011d62 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -229,7 +229,7 @@ public static void setupController(
                     
                       public static void setupAssistantController(Drive drive, CommandXboxController controller) {
                         controller
-                            .x()
+                            .rightTrigger()
                             .whileTrue(
                                 new RunCommand(
                                     () -> {
@@ -247,7 +247,15 @@ public static void setupAssistantController(Drive drive, CommandXboxController c
                                       SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
                                       SmartDashboard.putString(
                                           "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
-                                    }));
+                                    }))
+                                    .onFalse(new InstantCommand(() -> {
+                                        myCoolPoseKeyIdx = coolestNumberEver + 7;
+                                        if (myCoolPoseKeyIdx > 14) {
+                                            myCoolPoseKeyIdx -= 12;
+                                        }
+                                        SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
+                                    
+                      }));
                       }
                     
                       private static Entry<Integer, Command> coolDynamicPathScoringCommand(

From c9219e506e5d18c03061872040e2fc17ada75228 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Sat, 8 Mar 2025 16:05:06 -0500
Subject: [PATCH 50/90] update intake and score

---
 .../subsystems/controls/combination/DriverControls.java  | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index d9a68ac6..8fd201c1 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -14,8 +14,11 @@
 import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.elevator.ElevatorCommand;
 import frc.robot.commands.common.elevator.ElevatorToPosition;
+import frc.robot.commands.common.motor.MotorAutoResetEncoderCommand;
+import frc.robot.subsystems.controls.arm.CoralArmControls;
 import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Elevator;
+import frc.robot.subsystems.interfaces.Motor;
 import frc.robot.subsystems.interfaces.SimpleMotor;
 import java.util.Map;
 
@@ -44,7 +47,9 @@ public static void setupController(
     controller
         .b()
         .onTrue(
-            prepareIntakeCoralCommand.andThen(
+            prepareIntakeCoralCommand
+            .andThen(new MotorAutoResetEncoderCommand((Motor) coralArm, CoralArmControls.Constants.autoZeroSettings))
+            .andThen(
                 new InstantCommand(
                     () -> {
                       DriverControls.Constants.prepareScoreSelctedIndex = 1;
@@ -57,7 +62,7 @@ public static void setupController(
     Trigger scoreMode = new Trigger(() -> Constants.prepareScoreSelctedIndex >= 2);
     controller.leftTrigger().and(scoreMode.negate()).and(ableToIntake).onTrue(intakeCoralCommand);
 
-    Command score = new ArmToPosition(coralArm, () -> 0);
+    Command score = new ArmToPosition(coralArm, () -> -10);
     controller.rightBumper().onTrue(score);
 
     Constants.prepareChooser.setDefaultOption("L2", 2);

From 2317f54de2ea5c50c913a343a81289acd03fe0c8 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Sat, 8 Mar 2025 16:53:16 -0500
Subject: [PATCH 51/90] Slowed Down G Auto

---
 .../autos/Score F L4 + Y + C L4 .auto         |   2 +-
 src/main/deploy/pathplanner/paths/Back C.path |  12 +-
 src/main/deploy/pathplanner/paths/Back F.path |  16 +-
 src/main/deploy/pathplanner/paths/Back G.path |   4 +-
 src/main/deploy/pathplanner/paths/Back I.path |  20 +--
 src/main/deploy/pathplanner/paths/Back L.path |  12 +-
 .../pathplanner/paths/F to Source X.path      |   4 +-
 .../pathplanner/paths/G to Source Y.path      |   4 +-
 .../deploy/pathplanner/paths/Get to F.path    |  12 +-
 .../deploy/pathplanner/paths/Get to G.path    |   6 +-
 .../deploy/pathplanner/paths/Get to I.path    |  20 +--
 .../deploy/pathplanner/paths/Go Forward.path  |   4 +-
 .../pathplanner/paths/I to Source Y.path      |  12 +-
 .../pathplanner/paths/Source X to C.path      |  12 +-
 .../pathplanner/paths/Source Y to L.path      |  12 +-
 .../pathplanner/paths/Start to Near F.path    |   8 +-
 .../pathplanner/paths/Start to Near G.path    |   6 +-
 .../pathplanner/paths/Start to Near I.path    |  12 +-
 src/main/deploy/pathplanner/settings.json     |   4 +-
 .../controls/drive/DriveControls.java         | 166 +++++++++---------
 20 files changed, 174 insertions(+), 174 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto
index bc8c86d9..70b063a9 100644
--- a/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto	
@@ -101,7 +101,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 4.0
+            "waitTime": 3.0
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/paths/Back C.path b/src/main/deploy/pathplanner/paths/Back C.path
index e654c08d..777ec0e3 100644
--- a/src/main/deploy/pathplanner/paths/Back C.path	
+++ b/src/main/deploy/pathplanner/paths/Back C.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.6595738636363637,
-        "y": 2.943082386363636
+        "x": 3.729374999999999,
+        "y": 3.0627414772727275
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.6156440319325927,
-        "y": 2.6969723095961897
+        "x": 3.685445168296228,
+        "y": 2.816631400505281
       },
       "isLocked": false,
       "linkedName": "C Reef"
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/Back F.path b/src/main/deploy/pathplanner/paths/Back F.path
index c3750a7e..c4369369 100644
--- a/src/main/deploy/pathplanner/paths/Back F.path	
+++ b/src/main/deploy/pathplanner/paths/Back F.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.265,
-        "y": 2.933110795454545
+        "x": 5.2350852272727275,
+        "y": 3.0627414772727275
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.544368285561805,
-        "y": 2.804956864154667
+        "x": 5.469771343104384,
+        "y": 2.624743314985711
       },
       "isLocked": false,
       "linkedName": "F Reef"
@@ -20,8 +20,8 @@
         "y": 2.77356534090909
       },
       "prevControl": {
-        "x": 5.019288168232361,
-        "y": 2.8766904771262314
+        "x": 5.275994919451355,
+        "y": 2.9936234148069625
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/Back G.path b/src/main/deploy/pathplanner/paths/Back G.path
index 5ef72008..7c7187e8 100644
--- a/src/main/deploy/pathplanner/paths/Back G.path	
+++ b/src/main/deploy/pathplanner/paths/Back G.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/Back I.path b/src/main/deploy/pathplanner/paths/Back I.path
index 9f9ceae7..639af651 100644
--- a/src/main/deploy/pathplanner/paths/Back I.path	
+++ b/src/main/deploy/pathplanner/paths/Back I.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.354744318181819,
-        "y": 5.017173295454545
+        "x": 5.2350852272727275,
+        "y": 4.9972301136363635
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.596402748185905,
-        "y": 5.478657531140626
+        "x": 5.3576505371808665,
+        "y": 5.215123997917501
       },
       "isLocked": false,
       "linkedName": "I Reef"
     },
     {
       "anchor": {
-        "x": 5.544204545454546,
-        "y": 5.376150568181817
+        "x": 5.4046022727272724,
+        "y": 5.28640625
       },
       "prevControl": {
-        "x": 5.318787094807959,
-        "y": 4.950391851822856
+        "x": 5.248428510838665,
+        "y": 5.091189047639243
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/Back L.path b/src/main/deploy/pathplanner/paths/Back L.path
index c3586808..dbe6fe32 100644
--- a/src/main/deploy/pathplanner/paths/Back L.path	
+++ b/src/main/deploy/pathplanner/paths/Back L.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.61,
-        "y": 5.17671875
+        "x": 3.7194034090909094,
+        "y": 4.9972301136363635
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.4025986903265286,
-        "y": 5.42623827863789
+        "x": 3.512002099417438,
+        "y": 5.246749642274254
       },
       "isLocked": false,
       "linkedName": "L Reef"
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/F to Source X.path b/src/main/deploy/pathplanner/paths/F to Source X.path
index 7afb4b08..a4e8bef4 100644
--- a/src/main/deploy/pathplanner/paths/F to Source X.path	
+++ b/src/main/deploy/pathplanner/paths/F to Source X.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/G to Source Y.path b/src/main/deploy/pathplanner/paths/G to Source Y.path
index 51bb6504..86d90ec6 100644
--- a/src/main/deploy/pathplanner/paths/G to Source Y.path	
+++ b/src/main/deploy/pathplanner/paths/G to Source Y.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/Get to F.path b/src/main/deploy/pathplanner/paths/Get to F.path
index efcc9d7e..a02fe26c 100644
--- a/src/main/deploy/pathplanner/paths/Get to F.path	
+++ b/src/main/deploy/pathplanner/paths/Get to F.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 5.265,
-        "y": 2.933110795454545
+        "x": 5.2350852272727275,
+        "y": 3.0627414772727275
       },
       "prevControl": {
-        "x": 5.453247064609036,
-        "y": 2.7490837110619077
+        "x": 5.4233322918817635,
+        "y": 2.8787143928800902
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/Get to G.path b/src/main/deploy/pathplanner/paths/Get to G.path
index 6e04fcae..3449dc69 100644
--- a/src/main/deploy/pathplanner/paths/Get to G.path	
+++ b/src/main/deploy/pathplanner/paths/Get to G.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to I.path b/src/main/deploy/pathplanner/paths/Get to I.path
index c06f51ac..2dd5361a 100644
--- a/src/main/deploy/pathplanner/paths/Get to I.path	
+++ b/src/main/deploy/pathplanner/paths/Get to I.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.544204545454546,
-        "y": 5.376150568181817
+        "x": 5.4046022727272724,
+        "y": 5.28640625
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.39185046049515,
-        "y": 5.1779379732334005
+        "x": 5.252248187767877,
+        "y": 5.088193655051583
       },
       "isLocked": false,
       "linkedName": "Near I"
     },
     {
       "anchor": {
-        "x": 5.354744318181819,
-        "y": 5.017173295454545
+        "x": 5.2350852272727275,
+        "y": 4.9972301136363635
       },
       "prevControl": {
-        "x": 5.517467341821426,
-        "y": 5.213705553302059
+        "x": 5.397808250912335,
+        "y": 5.193762371483878
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/Go Forward.path b/src/main/deploy/pathplanner/paths/Go Forward.path
index e2d42621..3c0ecec6 100644
--- a/src/main/deploy/pathplanner/paths/Go Forward.path	
+++ b/src/main/deploy/pathplanner/paths/Go Forward.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/I to Source Y.path b/src/main/deploy/pathplanner/paths/I to Source Y.path
index f7a00bb9..d0f00fc6 100644
--- a/src/main/deploy/pathplanner/paths/I to Source Y.path	
+++ b/src/main/deploy/pathplanner/paths/I to Source Y.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.544204545454546,
-        "y": 5.376150568181817
+        "x": 5.4046022727272724,
+        "y": 5.28640625
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.810596590904106,
-        "y": 6.542826704540467
+        "x": 6.670994318176833,
+        "y": 6.453082386358649
       },
       "isLocked": false,
       "linkedName": "Near I"
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/Source X to C.path b/src/main/deploy/pathplanner/paths/Source X to C.path
index ef965bb1..76dc2aba 100644
--- a/src/main/deploy/pathplanner/paths/Source X to C.path	
+++ b/src/main/deploy/pathplanner/paths/Source X to C.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 3.6595738636363637,
-        "y": 2.943082386363636
+        "x": 3.729374999999999,
+        "y": 3.0627414772727275
       },
       "prevControl": {
-        "x": 3.3196597852791605,
-        "y": 2.661497390465073
+        "x": 3.389460921642796,
+        "y": 2.7811564813741643
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/Source Y to L.path b/src/main/deploy/pathplanner/paths/Source Y to L.path
index 50a68e7c..f984b380 100644
--- a/src/main/deploy/pathplanner/paths/Source Y to L.path	
+++ b/src/main/deploy/pathplanner/paths/Source Y to L.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 3.61,
-        "y": 5.17671875
+        "x": 3.7194034090909094,
+        "y": 4.9972301136363635
       },
       "prevControl": {
-        "x": 3.2973048614739695,
-        "y": 5.441841382452913
+        "x": 3.406708270564879,
+        "y": 5.2623527460892765
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/Start to Near F.path b/src/main/deploy/pathplanner/paths/Start to Near F.path
index afba264d..16f91c30 100644
--- a/src/main/deploy/pathplanner/paths/Start to Near F.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Near F.path	
@@ -20,8 +20,8 @@
         "y": 2.77356534090909
       },
       "prevControl": {
-        "x": 5.874725597416754,
-        "y": 2.6152866739994796
+        "x": 5.893210227272728,
+        "y": 2.1952130681818187
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/Start to Near G.path b/src/main/deploy/pathplanner/paths/Start to Near G.path
index 6519c3b1..edbe03bd 100644
--- a/src/main/deploy/pathplanner/paths/Start to Near G.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Near G.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to Near I.path b/src/main/deploy/pathplanner/paths/Start to Near I.path
index 0d8f83eb..361ce45e 100644
--- a/src/main/deploy/pathplanner/paths/Start to Near I.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Near I.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 5.544204545454546,
-        "y": 5.376150568181817
+        "x": 5.4046022727272724,
+        "y": 5.28640625
       },
       "prevControl": {
-        "x": 5.896167484058749,
-        "y": 5.4938919104517545
+        "x": 5.604034090909091,
+        "y": 5.874730113636364
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 45432d94..c966c029 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -4,8 +4,8 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 2.5,
-  "defaultMaxAccel": 2.5,
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
   "defaultMaxAngVel": 90.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 44f1c491..f8b87eea 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -186,89 +186,89 @@ public static void setupController(
                     constraints,
                     getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0))),
-                                () -> {
-                                  return myCoolPoseKeyIdx;
-                                });
-                    
-                        // TODO: new dynamic path planning command creation
-                        // 1. SelectCommand ONE: go to initial location (based on user-chosen option X)
-                        // 2. SelectCommand TWO: determine next action (based on option X and potentially pre-decided
-                        // reef elevation value)
-                        // 2a. if (1) was a reef position, assume we are scoring --> get reef elevation to score on -->
-                        // set elevator/arm
-                        // 2b. if not, choose an empty command and short-circuit (exit) the sequence
-                        // 3. SelectCommand THREE: go to scoring position (which maps directly from initial chosen
-                        // location)
-                        // 4. Execute <arm down command>
-                    
-                        // dynamically go to destination
-                        controller.rightTrigger().whileTrue(coolGoToPose);
-                    
-                        // TEMP FUNCTION TO TEST FIELD FLIPPING
-                        // controller.b().whileTrue(new SequentialCommandGroup(
-                        //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPrepPose(), constraints, 0.0),
-                        //     // DriverControls.Constants.prepareScoreCommand,
-                        //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPose(), constraints, 0.0),
-                        //     new ArmToPosition(arm, () -> 0)));
-                    
-                        /*  Angles -> Reef positions
-                         * 0    30  : E
-                         * 30   60  : F
-                         * 60   90  : G
-                         * 90   120 : H
-                         * 120  150 : I
-                         * 150  180 : J
-                         * 180  210 : K
-                         * 210  240 : L
-                         * 240  270 : A
-                         * 270  300 : B
-                         * 300  330 : C
-                         * 330  360 : D
-                         */
-                      }
-                    
-                      public static void setupAssistantController(Drive drive, CommandXboxController controller) {
-                        controller
-                            .x()
-                            .whileTrue(
-                                new RunCommand(
-                                    () -> {
-                                      double myX = controller.getLeftX();
-                                      double myY = -controller.getLeftY();
-                    
-                                      // TODO maybe keep calculation in radians?
-                                      double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
-                                      if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
-                    
-                                      // This number can be used to index into the ordered reef positions array!
-                                      coolestNumberEver = (int) myNumber / 30;
-                    
-                                      // TODO remove Smartdashboard number; only display reef position
-                                      SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
-                                      SmartDashboard.putString(
-                                          "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
-                                    }));
-                      }
-                    
-                      private static Entry<Integer, Command> coolDynamicPathScoringCommand(
-                          TargetPose target,
-                          PathConstraints constraints,
-                          Command prepareToScoreCommand,
-                          Command scoreCommand) {
-                        // FIXME initialize a fresh instance of the prepareToScoreCommand each time!!
-                        // OTHERWISE CODE WILL CRASH
-                        Entry<Integer, Command> entry =
-                            Map.entry(
-                                target.getIndex(),
-                                new SequentialCommandGroup(
-                                    AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
-                                    prepareToScoreCommand,
-                                    AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
-                                    scoreCommand));
-                        return entry;
-                      }
-                    
-    private static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm) {
+            () -> {
+              return myCoolPoseKeyIdx;
+            });
+
+    // TODO: new dynamic path planning command creation
+    // 1. SelectCommand ONE: go to initial location (based on user-chosen option X)
+    // 2. SelectCommand TWO: determine next action (based on option X and potentially pre-decided
+    // reef elevation value)
+    // 2a. if (1) was a reef position, assume we are scoring --> get reef elevation to score on -->
+    // set elevator/arm
+    // 2b. if not, choose an empty command and short-circuit (exit) the sequence
+    // 3. SelectCommand THREE: go to scoring position (which maps directly from initial chosen
+    // location)
+    // 4. Execute <arm down command>
+
+    // dynamically go to destination
+    controller.rightTrigger().whileTrue(coolGoToPose);
+
+    // TEMP FUNCTION TO TEST FIELD FLIPPING
+    // controller.b().whileTrue(new SequentialCommandGroup(
+    //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPrepPose(), constraints, 0.0),
+    //     // DriverControls.Constants.prepareScoreCommand,
+    //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPose(), constraints, 0.0),
+    //     new ArmToPosition(arm, () -> 0)));
+
+    /*  Angles -> Reef positions
+     * 0    30  : E
+     * 30   60  : F
+     * 60   90  : G
+     * 90   120 : H
+     * 120  150 : I
+     * 150  180 : J
+     * 180  210 : K
+     * 210  240 : L
+     * 240  270 : A
+     * 270  300 : B
+     * 300  330 : C
+     * 330  360 : D
+     */
+  }
+
+  public static void setupAssistantController(Drive drive, CommandXboxController controller) {
+    controller
+        .x()
+        .whileTrue(
+            new RunCommand(
+                () -> {
+                  double myX = controller.getLeftX();
+                  double myY = -controller.getLeftY();
+
+                  // TODO maybe keep calculation in radians?
+                  double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
+                  if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
+
+                  // This number can be used to index into the ordered reef positions array!
+                  coolestNumberEver = (int) myNumber / 30;
+
+                  // TODO remove Smartdashboard number; only display reef position
+                  SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
+                  SmartDashboard.putString(
+                      "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
+                }));
+  }
+
+  private static Entry<Integer, Command> coolDynamicPathScoringCommand(
+      TargetPose target,
+      PathConstraints constraints,
+      Command prepareToScoreCommand,
+      Command scoreCommand) {
+    // FIXME initialize a fresh instance of the prepareToScoreCommand each time!!
+    // OTHERWISE CODE WILL CRASH
+    Entry<Integer, Command> entry =
+        Map.entry(
+            target.getIndex(),
+            new SequentialCommandGroup(
+                AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
+                prepareToScoreCommand,
+                AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
+                scoreCommand));
+    return entry;
+  }
+
+  private static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm) {
     return new SelectCommand<>(
         Map.ofEntries(
             Map.entry(1, new InstantCommand(() -> System.out.println("Not scoring mode selected"))),

From 50e0b2b79af758440327d01429ddb6c0bde5c846 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Sat, 8 Mar 2025 17:25:46 -0500
Subject: [PATCH 52/90] trimming navgrid

---
 src/main/deploy/pathplanner/navgrid.json      |   2 +-
 .../controls/drive/DriveControls.java         | 183 +++++++++---------
 2 files changed, 93 insertions(+), 92 deletions(-)

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index adb2d744..9b6076a4 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index ec011d62..a811472f 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -186,97 +186,98 @@ public static void setupController(
                     constraints,
                     getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0))),
-                                () -> {
-                                  return myCoolPoseKeyIdx;
-                                });
-                    
-                        // TODO: new dynamic path planning command creation
-                        // 1. SelectCommand ONE: go to initial location (based on user-chosen option X)
-                        // 2. SelectCommand TWO: determine next action (based on option X and potentially pre-decided
-                        // reef elevation value)
-                        // 2a. if (1) was a reef position, assume we are scoring --> get reef elevation to score on -->
-                        // set elevator/arm
-                        // 2b. if not, choose an empty command and short-circuit (exit) the sequence
-                        // 3. SelectCommand THREE: go to scoring position (which maps directly from initial chosen
-                        // location)
-                        // 4. Execute <arm down command>
-                    
-                        // dynamically go to destination
-                        controller.rightTrigger().whileTrue(coolGoToPose);
-                    
-                        // TEMP FUNCTION TO TEST FIELD FLIPPING
-                        // controller.b().whileTrue(new SequentialCommandGroup(
-                        //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPrepPose(), constraints, 0.0),
-                        //     // DriverControls.Constants.prepareScoreCommand,
-                        //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPose(), constraints, 0.0),
-                        //     new ArmToPosition(arm, () -> 0)));
-                    
-                        /*  Angles -> Reef positions
-                         * 0    30  : E
-                         * 30   60  : F
-                         * 60   90  : G
-                         * 90   120 : H
-                         * 120  150 : I
-                         * 150  180 : J
-                         * 180  210 : K
-                         * 210  240 : L
-                         * 240  270 : A
-                         * 270  300 : B
-                         * 300  330 : C
-                         * 330  360 : D
-                         */
-                      }
-                    
-                      public static void setupAssistantController(Drive drive, CommandXboxController controller) {
-                        controller
-                            .rightTrigger()
-                            .whileTrue(
-                                new RunCommand(
-                                    () -> {
-                                      double myX = controller.getLeftX();
-                                      double myY = -controller.getLeftY();
-                    
-                                      // TODO maybe keep calculation in radians?
-                                      double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
-                                      if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
-                    
-                                      // This number can be used to index into the ordered reef positions array!
-                                      coolestNumberEver = (int) myNumber / 30;
-                    
-                                      // TODO remove Smartdashboard number; only display reef position
-                                      SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
-                                      SmartDashboard.putString(
-                                          "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
-                                    }))
-                                    .onFalse(new InstantCommand(() -> {
-                                        myCoolPoseKeyIdx = coolestNumberEver + 7;
-                                        if (myCoolPoseKeyIdx > 14) {
-                                            myCoolPoseKeyIdx -= 12;
-                                        }
-                                        SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
-                                    
-                      }));
-                      }
-                    
-                      private static Entry<Integer, Command> coolDynamicPathScoringCommand(
-                          TargetPose target,
-                          PathConstraints constraints,
-                          Command prepareToScoreCommand,
-                          Command scoreCommand) {
-                        // FIXME initialize a fresh instance of the prepareToScoreCommand each time!!
-                        // OTHERWISE CODE WILL CRASH
-                        Entry<Integer, Command> entry =
-                            Map.entry(
-                                target.getIndex(),
-                                new SequentialCommandGroup(
-                                    AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
-                                    prepareToScoreCommand,
-                                    AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
-                                    scoreCommand));
-                        return entry;
-                      }
-                    
-    private static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm) {
+            () -> {
+              return myCoolPoseKeyIdx;
+            });
+
+    // TODO: new dynamic path planning command creation
+    // 1. SelectCommand ONE: go to initial location (based on user-chosen option X)
+    // 2. SelectCommand TWO: determine next action (based on option X and potentially pre-decided
+    // reef elevation value)
+    // 2a. if (1) was a reef position, assume we are scoring --> get reef elevation to score on -->
+    // set elevator/arm
+    // 2b. if not, choose an empty command and short-circuit (exit) the sequence
+    // 3. SelectCommand THREE: go to scoring position (which maps directly from initial chosen
+    // location)
+    // 4. Execute <arm down command>
+
+    // dynamically go to destination
+    controller.rightTrigger().whileTrue(coolGoToPose);
+
+    // TEMP FUNCTION TO TEST FIELD FLIPPING
+    // controller.b().whileTrue(new SequentialCommandGroup(
+    //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPrepPose(), constraints, 0.0),
+    //     // DriverControls.Constants.prepareScoreCommand,
+    //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPose(), constraints, 0.0),
+    //     new ArmToPosition(arm, () -> 0)));
+
+    /*  Angles -> Reef positions
+     * 0    30  : E
+     * 30   60  : F
+     * 60   90  : G
+     * 90   120 : H
+     * 120  150 : I
+     * 150  180 : J
+     * 180  210 : K
+     * 210  240 : L
+     * 240  270 : A
+     * 270  300 : B
+     * 300  330 : C
+     * 330  360 : D
+     */
+  }
+
+  public static void setupAssistantController(Drive drive, CommandXboxController controller) {
+    controller
+        .rightTrigger()
+        .whileTrue(
+            new RunCommand(
+                () -> {
+                  double myX = controller.getLeftX();
+                  double myY = -controller.getLeftY();
+
+                  // TODO maybe keep calculation in radians?
+                  double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
+                  if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
+
+                  // This number can be used to index into the ordered reef positions array!
+                  coolestNumberEver = (int) myNumber / 30;
+
+                  // TODO remove Smartdashboard number; only display reef position
+                  SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
+                  SmartDashboard.putString(
+                      "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
+                }))
+        .onFalse(
+            new InstantCommand(
+                () -> {
+                  myCoolPoseKeyIdx = coolestNumberEver + 7;
+                  if (myCoolPoseKeyIdx > 14) {
+                    myCoolPoseKeyIdx -= 12;
+                  }
+                  SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
+                }));
+  }
+
+  private static Entry<Integer, Command> coolDynamicPathScoringCommand(
+      TargetPose target,
+      PathConstraints constraints,
+      Command prepareToScoreCommand,
+      Command scoreCommand) {
+    // FIXME initialize a fresh instance of the prepareToScoreCommand each time!!
+    // OTHERWISE CODE WILL CRASH
+    Entry<Integer, Command> entry =
+        Map.entry(
+            target.getIndex(),
+            new SequentialCommandGroup(
+                AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
+                prepareToScoreCommand,
+                AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
+                scoreCommand));
+    return entry;
+  }
+
+  private static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm) {
     return new SelectCommand<>(
         Map.ofEntries(
             Map.entry(1, new InstantCommand(() -> System.out.println("Not scoring mode selected"))),

From 62927e7336bb81e6dcbd05a31612f77880108608 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Sat, 8 Mar 2025 17:36:05 -0500
Subject: [PATCH 53/90] Slowed down I and F paths

---
 .../deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto    | 2 +-
 src/main/deploy/pathplanner/paths/Back I.path               | 6 +++---
 src/main/deploy/pathplanner/paths/Get to F.path             | 6 +++---
 src/main/deploy/pathplanner/paths/Get to I.path             | 6 +++---
 src/main/deploy/pathplanner/paths/Start to Near F.path      | 6 +++---
 src/main/deploy/pathplanner/paths/Start to Near I.path      | 6 +++---
 6 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto
index 7bd9e6a7..fb80f729 100644
--- a/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
@@ -101,7 +101,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 4.0
+            "waitTime": 3.0
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/paths/Back I.path b/src/main/deploy/pathplanner/paths/Back I.path
index 639af651..7c3d6447 100644
--- a/src/main/deploy/pathplanner/paths/Back I.path	
+++ b/src/main/deploy/pathplanner/paths/Back I.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": -119.99999999999999
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to F.path b/src/main/deploy/pathplanner/paths/Get to F.path
index a02fe26c..f7f8a718 100644
--- a/src/main/deploy/pathplanner/paths/Get to F.path	
+++ b/src/main/deploy/pathplanner/paths/Get to F.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 122.66091272167375
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to I.path b/src/main/deploy/pathplanner/paths/Get to I.path
index 2dd5361a..0f4d2056 100644
--- a/src/main/deploy/pathplanner/paths/Get to I.path	
+++ b/src/main/deploy/pathplanner/paths/Get to I.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": -119.99999999999999
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to Near F.path b/src/main/deploy/pathplanner/paths/Start to Near F.path
index 16f91c30..6de63a0f 100644
--- a/src/main/deploy/pathplanner/paths/Start to Near F.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Near F.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to Near I.path b/src/main/deploy/pathplanner/paths/Start to Near I.path
index 361ce45e..cc9f00af 100644
--- a/src/main/deploy/pathplanner/paths/Start to Near I.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Near I.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file

From 7b2d50e341e2042abb3de2c52fa00b57a983e7db Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Sat, 8 Mar 2025 17:41:07 -0500
Subject: [PATCH 54/90] set default scoring level for dynamic pathplanning and
 more navgrid edits

---
 src/main/deploy/pathplanner/navgrid.json                   | 2 +-
 .../frc/robot/subsystems/controls/drive/DriveControls.java | 7 ++++++-
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 9b6076a4..15e858d3 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index a811472f..c9e1592e 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -280,7 +280,12 @@ private static Entry<Integer, Command> coolDynamicPathScoringCommand(
   private static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm) {
     return new SelectCommand<>(
         Map.ofEntries(
-            Map.entry(1, new InstantCommand(() -> System.out.println("Not scoring mode selected"))),
+            Map.entry(
+                1,
+                new SequentialCommandGroup(
+                        new ElevatorToPosition(elevator, () -> 0.63),
+                        new ArmToPosition(coralArm, () -> 47))
+                    .withTimeout(5.0)),
             Map.entry(
                 2,
                 new SequentialCommandGroup(

From fb0eaadc753a36c36276d40250b2b496ec8c1a8e Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Sat, 8 Mar 2025 19:36:33 -0500
Subject: [PATCH 55/90] Slowed down rotation

---
 .../autos/Score J L4 + Y + L L4 .auto         | 176 ++++++++++++++++++
 .../deploy/pathplanner/autos/Score J L4.auto  |  82 ++++++++
 src/main/deploy/pathplanner/paths/Back C.path |   2 +-
 src/main/deploy/pathplanner/paths/Back F.path |   2 +-
 src/main/deploy/pathplanner/paths/Back G.path |   2 +-
 src/main/deploy/pathplanner/paths/Back J.path |  54 ++++++
 src/main/deploy/pathplanner/paths/Back L.path |   2 +-
 .../pathplanner/paths/F to Source X.path      |   2 +-
 .../pathplanner/paths/G to Source Y.path      |   2 +-
 .../deploy/pathplanner/paths/Get to J.path    |  54 ++++++
 .../deploy/pathplanner/paths/Go Forward.path  |   2 +-
 .../pathplanner/paths/I to Source Y.path      |   2 +-
 .../pathplanner/paths/Source X to C.path      |   2 +-
 .../pathplanner/paths/Source Y to L.path      |   2 +-
 .../pathplanner/paths/Start to Near J.path    |  54 ++++++
 src/main/deploy/pathplanner/settings.json     |   2 +-
 16 files changed, 431 insertions(+), 11 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto
 create mode 100644 src/main/deploy/pathplanner/autos/Score J L4.auto
 create mode 100644 src/main/deploy/pathplanner/paths/Back J.path
 create mode 100644 src/main/deploy/pathplanner/paths/Get to J.path
 create mode 100644 src/main/deploy/pathplanner/paths/Start to Near J.path

diff --git a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto
new file mode 100644
index 00000000..04e7269b
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
@@ -0,0 +1,176 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near J"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to J"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back J"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "I to Source Y"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Elevator to 0.8 meter"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Arm for Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Elevator to 0.4 meter"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Source Y to L"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.4 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back L"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score J L4.auto b/src/main/deploy/pathplanner/autos/Score J L4.auto
new file mode 100644
index 00000000..2836fd08
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score J L4.auto	
@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near J"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to J"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back J"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back C.path b/src/main/deploy/pathplanner/paths/Back C.path
index 777ec0e3..e5ffa71a 100644
--- a/src/main/deploy/pathplanner/paths/Back C.path	
+++ b/src/main/deploy/pathplanner/paths/Back C.path	
@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 90.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/Back F.path b/src/main/deploy/pathplanner/paths/Back F.path
index c4369369..cf1474a9 100644
--- a/src/main/deploy/pathplanner/paths/Back F.path	
+++ b/src/main/deploy/pathplanner/paths/Back F.path	
@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 90.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/Back G.path b/src/main/deploy/pathplanner/paths/Back G.path
index 7c7187e8..f9d2627c 100644
--- a/src/main/deploy/pathplanner/paths/Back G.path	
+++ b/src/main/deploy/pathplanner/paths/Back G.path	
@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 90.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/Back J.path b/src/main/deploy/pathplanner/paths/Back J.path
new file mode 100644
index 00000000..07ea5fc9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Back J.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.925965909090909,
+        "y": 5.206633522727272
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.0454673590505665,
+        "y": 5.4262226924454975
+      },
+      "isLocked": false,
+      "linkedName": "J Reef"
+    },
+    {
+      "anchor": {
+        "x": 5.045624999999999,
+        "y": 5.376150568181817
+      },
+      "prevControl": {
+        "x": 4.923232686600558,
+        "y": 5.147387493000958
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near J"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back L.path b/src/main/deploy/pathplanner/paths/Back L.path
index dbe6fe32..77214bf4 100644
--- a/src/main/deploy/pathplanner/paths/Back L.path	
+++ b/src/main/deploy/pathplanner/paths/Back L.path	
@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 90.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/F to Source X.path b/src/main/deploy/pathplanner/paths/F to Source X.path
index a4e8bef4..eb34c20c 100644
--- a/src/main/deploy/pathplanner/paths/F to Source X.path	
+++ b/src/main/deploy/pathplanner/paths/F to Source X.path	
@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 90.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/G to Source Y.path b/src/main/deploy/pathplanner/paths/G to Source Y.path
index 86d90ec6..77b5293d 100644
--- a/src/main/deploy/pathplanner/paths/G to Source Y.path	
+++ b/src/main/deploy/pathplanner/paths/G to Source Y.path	
@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 90.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/Get to J.path b/src/main/deploy/pathplanner/paths/Get to J.path
new file mode 100644
index 00000000..92d5a80b
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Get to J.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.045624999999999,
+        "y": 5.376150568181817
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.895624999999999,
+        "y": 5.176150568181817
+      },
+      "isLocked": false,
+      "linkedName": "Near J"
+    },
+    {
+      "anchor": {
+        "x": 4.925965909090909,
+        "y": 5.206633522727272
+      },
+      "prevControl": {
+        "x": 5.064640958147216,
+        "y": 5.414646096311733
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "J Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Go Forward.path b/src/main/deploy/pathplanner/paths/Go Forward.path
index 3c0ecec6..de3846ef 100644
--- a/src/main/deploy/pathplanner/paths/Go Forward.path	
+++ b/src/main/deploy/pathplanner/paths/Go Forward.path	
@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 90.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/I to Source Y.path b/src/main/deploy/pathplanner/paths/I to Source Y.path
index d0f00fc6..a7115930 100644
--- a/src/main/deploy/pathplanner/paths/I to Source Y.path	
+++ b/src/main/deploy/pathplanner/paths/I to Source Y.path	
@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 90.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/Source X to C.path b/src/main/deploy/pathplanner/paths/Source X to C.path
index 76dc2aba..b2b156eb 100644
--- a/src/main/deploy/pathplanner/paths/Source X to C.path	
+++ b/src/main/deploy/pathplanner/paths/Source X to C.path	
@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 90.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/Source Y to L.path b/src/main/deploy/pathplanner/paths/Source Y to L.path
index f984b380..e2ff3f5d 100644
--- a/src/main/deploy/pathplanner/paths/Source Y to L.path	
+++ b/src/main/deploy/pathplanner/paths/Source Y to L.path	
@@ -35,7 +35,7 @@
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 90.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/Start to Near J.path b/src/main/deploy/pathplanner/paths/Start to Near J.path
new file mode 100644
index 00000000..7de88cc3
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Start to Near J.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.867585227272728,
+        "y": 6.183849431818182
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.7906534090909085,
+        "y": 6.443110795454545
+      },
+      "isLocked": false,
+      "linkedName": "Right Start"
+    },
+    {
+      "anchor": {
+        "x": 5.045624999999999,
+        "y": 5.376150568181817
+      },
+      "prevControl": {
+        "x": 5.414573863636363,
+        "y": 6.433139204545454
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near J"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index c966c029..c20d16ae 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -6,7 +6,7 @@
   "autoFolders": [],
   "defaultMaxVel": 3.0,
   "defaultMaxAccel": 3.0,
-  "defaultMaxAngVel": 90.0,
+  "defaultMaxAngVel": 45.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
   "robotMass": 74.088,

From d6420d68b8ea839bb08e628f9d12af6b93b7842d Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Sat, 8 Mar 2025 19:55:50 -0500
Subject: [PATCH 56/90] Changed Climber climb angle. Needs to be tested

---
 .../subsystems/controls/combination/DriverControls.java      | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index d9a68ac6..052ca9ef 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -129,10 +129,7 @@ public static void setupController(
     // climb
     controller
         .rightBumper()
-        .onTrue(
-            new InstantCommand(
-                () -> climber.setTargetPosition(climber.getSettings().minPositionInRads),
-                climberSubsystem));
+        .onTrue(new InstantCommand(() -> climber.setTargetPosition(5.5), climberSubsystem));
 
     // multi controll not workking in each subsystem inde
     controller.povUp().whileTrue(new ElevatorCommand(elevator, () -> 0.2));

From 6dec235aeadb16b0a4b0792a23476f43746ab883 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Sun, 9 Mar 2025 12:18:11 -0400
Subject: [PATCH 57/90] chang clim rad

---
 .../deploy/pathplanner/autos/New mob.auto     | 19 +++++++
 .../deploy/pathplanner/paths/New Path.path    | 54 +++++++++++++++++++
 .../combination/DriverAssistControls.java     |  2 +-
 .../controls/combination/DriverControls.java  |  2 +-
 4 files changed, 75 insertions(+), 2 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/New mob.auto
 create mode 100644 src/main/deploy/pathplanner/paths/New Path.path

diff --git a/src/main/deploy/pathplanner/autos/New mob.auto b/src/main/deploy/pathplanner/autos/New mob.auto
new file mode 100644
index 00000000..268147bb
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/New mob.auto	
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
new file mode 100644
index 00000000..37e02580
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.678125,
+        "y": 6.662485795454545
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.384659090909091,
+        "y": 6.183849431818182
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.427386363636364,
+        "y": 5.934559659090909
+      },
+      "prevControl": {
+        "x": 5.424545454545455,
+        "y": 6.1539346590909085
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 172.48157312055784
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -178.94559549647815
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
index f5616c30..eff1196c 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
@@ -115,7 +115,7 @@ public static void setupController(
         .rightBumper()
         .onTrue(
             new InstantCommand(
-                () -> climber.setTargetPosition(climber.getSettings().minPositionInRads),
+                () -> climber.setTargetPosition(5.0),
                 climberSubsystem));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index ffe4b709..72911a0b 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -134,7 +134,7 @@ public static void setupController(
     // climb
     controller
         .rightBumper()
-        .onTrue(new InstantCommand(() -> climber.setTargetPosition(5.5), climberSubsystem));
+        .onTrue(new InstantCommand(() -> climber.setTargetPosition(5.0), climberSubsystem));
 
     // multi controll not workking in each subsystem inde
     controller.povUp().whileTrue(new ElevatorCommand(elevator, () -> 0.2));

From 337e971cba2f2c852daf93ac37242c17aa1bae83 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Mon, 10 Mar 2025 18:33:14 -0400
Subject: [PATCH 58/90]

---
 .../pathplanner/autos/Score 1 G L4.auto       |  6 +++++
 src/main/deploy/pathplanner/paths/Back G.path |  4 +--
 .../deploy/pathplanner/paths/Get to G.path    |  4 +--
 .../combination/DriverAssistControls.java     |  5 +---
 .../controls/combination/DriverControls.java  | 26 ++++++++++---------
 5 files changed, 25 insertions(+), 20 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/Score 1 G L4.auto b/src/main/deploy/pathplanner/autos/Score 1 G L4.auto
index 276e2b6b..edfea468 100644
--- a/src/main/deploy/pathplanner/autos/Score 1 G L4.auto	
+++ b/src/main/deploy/pathplanner/autos/Score 1 G L4.auto	
@@ -61,6 +61,12 @@
             "pathName": "Get to G"
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/deploy/pathplanner/paths/Back G.path b/src/main/deploy/pathplanner/paths/Back G.path
index f9d2627c..5a16d5e2 100644
--- a/src/main/deploy/pathplanner/paths/Back G.path	
+++ b/src/main/deploy/pathplanner/paths/Back G.path	
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.723693181818183,
+        "x": 5.753607954545455,
         "y": 3.85
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.9956146180941134,
+        "x": 6.025529390821386,
         "y": 3.840199234078392
       },
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Get to G.path b/src/main/deploy/pathplanner/paths/Get to G.path
index 3449dc69..c3f2fb2b 100644
--- a/src/main/deploy/pathplanner/paths/Get to G.path	
+++ b/src/main/deploy/pathplanner/paths/Get to G.path	
@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 5.723693181818183,
+        "x": 5.753607954545455,
         "y": 3.85
       },
       "prevControl": {
-        "x": 5.972908363197984,
+        "x": 6.002823135925256,
         "y": 3.8697937709856585
       },
       "nextControl": null,
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
index eff1196c..6b9ce176 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
@@ -113,9 +113,6 @@ public static void setupController(
     // climb
     controller
         .rightBumper()
-        .onTrue(
-            new InstantCommand(
-                () -> climber.setTargetPosition(5.0),
-                climberSubsystem));
+        .onTrue(new InstantCommand(() -> climber.setTargetPosition(11.6), climberSubsystem));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 72911a0b..2d46cd7d 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -48,17 +48,19 @@ public static void setupController(
         .b()
         .onTrue(
             prepareIntakeCoralCommand
-            .andThen(new MotorAutoResetEncoderCommand((Motor) coralArm, CoralArmControls.Constants.autoZeroSettings))
-            .andThen(
-                new InstantCommand(
-                    () -> {
-                      DriverControls.Constants.prepareScoreSelctedIndex = 1;
-                      SmartDashboard.putNumber(
-                          "Driver " + "/Misc/Prepare Selection",
-                          DriverControls.Constants.prepareScoreSelctedIndex);
-                    })));
-
-    Command intakeCoralCommand = new ElevatorToPosition(elevator, () -> 0.3);
+                .andThen(
+                    new MotorAutoResetEncoderCommand(
+                        (Motor) coralArm, CoralArmControls.Constants.autoZeroSettings))
+                .andThen(
+                    new InstantCommand(
+                        () -> {
+                          DriverControls.Constants.prepareScoreSelctedIndex = 1;
+                          SmartDashboard.putNumber(
+                              "Driver " + "/Misc/Prepare Selection",
+                              DriverControls.Constants.prepareScoreSelctedIndex);
+                        })));
+
+    Command intakeCoralCommand = new ElevatorToPosition(elevator, () -> 0.345);
     Trigger scoreMode = new Trigger(() -> Constants.prepareScoreSelctedIndex >= 2);
     controller.leftTrigger().and(scoreMode.negate()).and(ableToIntake).onTrue(intakeCoralCommand);
 
@@ -134,7 +136,7 @@ public static void setupController(
     // climb
     controller
         .rightBumper()
-        .onTrue(new InstantCommand(() -> climber.setTargetPosition(5.0), climberSubsystem));
+        .onTrue(new InstantCommand(() -> climber.setTargetPosition(11.6), climberSubsystem));
 
     // multi controll not workking in each subsystem inde
     controller.povUp().whileTrue(new ElevatorCommand(elevator, () -> 0.2));

From 3a7c3a03d70392e8bb763ac06b5a53b29097a042 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Mon, 10 Mar 2025 18:36:02 -0400
Subject: [PATCH 59/90] Fixed G Auto, and  Climber, Elevator heights


From d6ef60025cadc8f106f9f62f3831bd80e89c5a20 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Mon, 10 Mar 2025 21:06:56 -0400
Subject: [PATCH 60/90] tune drivetrain encoder offsets, modify G&H, amend
 flipped pose logging

---
 src/main/deploy/elastic-layout.json           | 430 +++++++++++++-----
 .../deploy/yagsl/comp/modules/backleft.json   |   2 +-
 .../deploy/yagsl/comp/modules/backright.json  |   2 +-
 .../deploy/yagsl/comp/modules/frontleft.json  |   2 +-
 .../deploy/yagsl/comp/modules/frontright.json |   2 +-
 .../java/frc/robot/io/interfaces/DriveIO.java |   6 +-
 .../subsystems/controls/drive/TargetPose.java |   2 +-
 .../drive/DriveSwerveYAGSL.java               |   2 +-
 8 files changed, 325 insertions(+), 123 deletions(-)

diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json
index 4a536fc5..cf326c6c 100644
--- a/src/main/deploy/elastic-layout.json
+++ b/src/main/deploy/elastic-layout.json
@@ -215,6 +215,47 @@
               "period": 0.06,
               "data_type": "double"
             }
+          },
+          {
+            "title": "Pose choices",
+            "x": 1152.0,
+            "y": 192.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "ComboBox Chooser",
+            "properties": {
+              "topic": "/SmartDashboard/Pose choices",
+              "period": 0.06,
+              "sort_options": false
+            }
+          },
+          {
+            "title": "AAAAAA Reef Position",
+            "x": 1152.0,
+            "y": 384.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/AAAAAA Reef Position",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Chosen Pose Index",
+            "x": 832.0,
+            "y": 448.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Chosen Pose Index",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
           }
         ]
       }
@@ -1931,125 +1972,14 @@
     {
       "name": "Drive",
       "grid_layout": {
-        "layouts": [
-          {
-            "title": "rotationPID",
-            "x": 896.0,
-            "y": 256.0,
-            "width": 256.0,
-            "height": 256.0,
-            "type": "List Layout",
-            "properties": {
-              "label_position": "TOP"
-            },
-            "children": [
-              {
-                "title": "P",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/rotationPID/P",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "I",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/rotationPID/I",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "D",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/rotationPID/D",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              }
-            ]
-          },
-          {
-            "title": "translationPID",
-            "x": 896.0,
-            "y": 0.0,
-            "width": 256.0,
-            "height": 256.0,
-            "type": "List Layout",
-            "properties": {
-              "label_position": "TOP"
-            },
-            "children": [
-              {
-                "title": "P",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/translationPID/P",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "I",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/translationPID/I",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "D",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/translationPID/D",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              }
-            ]
-          }
-        ],
+        "layouts": [],
         "containers": [
           {
             "title": "Field",
             "x": 0.0,
             "y": 0.0,
-            "width": 896.0,
-            "height": 512.0,
+            "width": 640.0,
+            "height": 320.0,
             "type": "Field",
             "properties": {
               "topic": "/SmartDashboard/Field",
@@ -2063,6 +1993,278 @@
               "robot_color": 4294198070,
               "trajectory_color": 4294967295
             }
+          },
+          {
+            "title": "Reef Position",
+            "x": 640.0,
+            "y": 128.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/AAAAAA Reef Position",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Auto Calibrate Elevator",
+            "x": 256.0,
+            "y": 448.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Elevator[Coral]/Commands/Auto Calibrate Elevator",
+              "period": 0.06,
+              "show_type": true
+            }
+          },
+          {
+            "title": "Auto Calibrate Coral Arm",
+            "x": 0.0,
+            "y": 448.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Arm[Coral]/Commands/Auto Calibrate Coral Arm",
+              "period": 0.06,
+              "show_type": true
+            }
+          },
+          {
+            "title": "Arm Angle (deg)",
+            "x": 0.0,
+            "y": 576.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Arm[Coral]/PositionDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Elevetor Position (m)",
+            "x": 256.0,
+            "y": 576.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Elevator[Coral]/PositionMeters",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Blue X (m)",
+            "x": 640.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/PoseX",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Blue Y (m)",
+            "x": 768.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/PoseY",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Blue Rot (deg)",
+            "x": 896.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/PoseRotInDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Red X (m)",
+            "x": 1152.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/FlippedPoseX",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Red Y (m)",
+            "x": 1280.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/FlippedPoseY",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Red Rot (deg)",
+            "x": 1408.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/FlippedPoseRotInDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Reef Level Selected",
+            "x": 640.0,
+            "y": 256.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Driver /Misc/Prepare To Score Selection",
+              "period": 0.06,
+              "data_type": "double"
+            }
+          },
+          {
+            "title": "BatteryVoltage",
+            "x": 512.0,
+            "y": 448.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Voltage View",
+            "properties": {
+              "topic": "/AdvantageKit/SystemStats/BatteryVoltage",
+              "period": 0.06,
+              "data_type": "double",
+              "min_value": 4.0,
+              "max_value": 13.0,
+              "divisions": 5,
+              "inverted": false,
+              "orientation": "horizontal"
+            }
+          },
+          {
+            "title": "Alliance Color",
+            "x": 640.0,
+            "y": 0.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/FMSInfo/IsRedAlliance",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4294901760,
+              "false_color": 4278190335,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Arm[Coral]",
+            "x": 0.0,
+            "y": 320.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Subsystem",
+            "properties": {
+              "topic": "/SmartDashboard/Arm[Coral]",
+              "period": 0.06
+            }
+          },
+          {
+            "title": "Elevator[Coral]",
+            "x": 256.0,
+            "y": 320.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Subsystem",
+            "properties": {
+              "topic": "/SmartDashboard/Elevator[Coral]",
+              "period": 0.06
+            }
+          },
+          {
+            "title": "Is Field Orient",
+            "x": 512.0,
+            "y": 320.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/DriveSwerveYAGSL/FieldOrientedDrive",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "FRONT CAM",
+            "x": 832.0,
+            "y": 0.0,
+            "width": 448.0,
+            "height": 384.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/photonvision_Port_1184_Output_MJPEG_Server",
+              "period": 0.06,
+              "rotation_turns": 0,
+              "fps": 10,
+              "resolution": [
+                160.0,
+                120.0
+              ]
+            }
+          },
+          {
+            "title": "Distance to Front Tag",
+            "x": 1280.0,
+            "y": 0.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/VisionSubsystem/Distances To Target/front_cam",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
           }
         ]
       }
diff --git a/src/main/deploy/yagsl/comp/modules/backleft.json b/src/main/deploy/yagsl/comp/modules/backleft.json
index 37510797..15bdef3b 100644
--- a/src/main/deploy/yagsl/comp/modules/backleft.json
+++ b/src/main/deploy/yagsl/comp/modules/backleft.json
@@ -3,7 +3,7 @@
     "front": -12.125,
     "left": 12.125
   },
-  "absoluteEncoderOffset": 277.383,
+  "absoluteEncoderOffset": 268.383,
   "drive": {
     "type": "sparkmax_neo",
     "id": 12,
diff --git a/src/main/deploy/yagsl/comp/modules/backright.json b/src/main/deploy/yagsl/comp/modules/backright.json
index 13316823..5d3f0bf2 100644
--- a/src/main/deploy/yagsl/comp/modules/backright.json
+++ b/src/main/deploy/yagsl/comp/modules/backright.json
@@ -3,7 +3,7 @@
     "front": -12.125,
     "left": -12.125
   },
-  "absoluteEncoderOffset": 333.676,
+  "absoluteEncoderOffset": 336.445,
   "drive": {
     "type": "sparkmax_neo",
     "id": 14,
diff --git a/src/main/deploy/yagsl/comp/modules/frontleft.json b/src/main/deploy/yagsl/comp/modules/frontleft.json
index 40594f82..6e09bc44 100644
--- a/src/main/deploy/yagsl/comp/modules/frontleft.json
+++ b/src/main/deploy/yagsl/comp/modules/frontleft.json
@@ -3,7 +3,7 @@
     "front": 12.125,
     "left": 12.125
   },
-  "absoluteEncoderOffset": 251.016,
+  "absoluteEncoderOffset": 247.850,
   "drive": {
     "type": "sparkmax_neo",
     "id": 10,
diff --git a/src/main/deploy/yagsl/comp/modules/frontright.json b/src/main/deploy/yagsl/comp/modules/frontright.json
index 7c711308..4d886a60 100644
--- a/src/main/deploy/yagsl/comp/modules/frontright.json
+++ b/src/main/deploy/yagsl/comp/modules/frontright.json
@@ -3,7 +3,7 @@
     "front": 12.125,
     "left": -12.125
   },
-  "absoluteEncoderOffset": 41.836,
+  "absoluteEncoderOffset": 39.200,
   "drive": {
     "type": "sparkmax_neo",
     "id": 16,
diff --git a/src/main/java/frc/robot/io/interfaces/DriveIO.java b/src/main/java/frc/robot/io/interfaces/DriveIO.java
index 2a798041..d3887636 100644
--- a/src/main/java/frc/robot/io/interfaces/DriveIO.java
+++ b/src/main/java/frc/robot/io/interfaces/DriveIO.java
@@ -25,9 +25,9 @@ public void updateInputs(DriveIOInputs inputs, SwerveDrive swerveDrive) {
     inputs.poseY = inputs.pose.getTranslation().getY();
     inputs.poseRotInDegrees = inputs.pose.getRotation().getDegrees();
     inputs.flippedPose = FlippingUtil.flipFieldPose(inputs.pose);
-    inputs.flippedPoseX = inputs.pose.getTranslation().getX();
-    inputs.flippedPoseY = inputs.pose.getTranslation().getY();
-    inputs.flippedPoseRotInDegrees = inputs.pose.getRotation().getDegrees();
+    inputs.flippedPoseX = inputs.flippedPose.getTranslation().getX();
+    inputs.flippedPoseY = inputs.flippedPose.getTranslation().getY();
+    inputs.flippedPoseRotInDegrees = inputs.flippedPose.getRotation().getDegrees();
   }
   // Other methods for controlling the drive subsystem...
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 2143dc58..672f0394 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -18,7 +18,7 @@ public enum TargetPose {
   REEF_D(6, "D", "Reef D", new Pose2d(4.10, 2.76, Rotation2d.fromDegrees(60)), 1),
   REEF_E(7, "E", "Reef E", new Pose2d(5.00, 2.83, Rotation2d.fromDegrees(120)), -1),
   REEF_F(8, "F", "Reef F", new Pose2d(5.36, 3.00, Rotation2d.fromDegrees(122)), 1),
-  REEF_G(9, "G", "Reef G", new Pose2d(5.71, 4.00, Rotation2d.fromDegrees(183)), -1),
+  REEF_G(9, "G", "Reef G", new Pose2d(5.80, 3.75, Rotation2d.fromDegrees(183)), -1),
   REEF_H(10, "H", "Reef H", new Pose2d(5.80, 4.20, Rotation2d.fromDegrees(180)), 1),
   REEF_I(11, "I", "Reef I", new Pose2d(5.28, 5.06, Rotation2d.fromDegrees(-120)), -1),
   REEF_J(12, "J", "Reef J", new Pose2d(4.97, 5.28, Rotation2d.fromDegrees(-120)), 1),
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index c5f58671..d9790cfa 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -49,7 +49,7 @@ public DriveSwerveYAGSL(String configPath) {
     super("YAGSL");
     swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), configPath);
 
-    // \SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
+    // SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
     try {
       swerveDrive =
           new SwerveParser(swerveJsonDirectory)

From 7ee1bc93a519bd9ae38ca1d5d947f09c09ba0174 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Tue, 11 Mar 2025 17:54:05 -0400
Subject: [PATCH 61/90] transitioning left camera to be center camera, fixing
 dashboard layout, further trimming navgrid, fixing G,H positions

---
 src/main/deploy/elastic-layout.json           | 100 +++++++++++++-----
 src/main/deploy/pathplanner/navgrid.json      |   2 +-
 .../game/reefscape2025/RobotConfigComp.java   |  37 ++++---
 .../subsystems/controls/drive/TargetPose.java |  30 +++++-
 4 files changed, 125 insertions(+), 44 deletions(-)

diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json
index cf326c6c..f1043190 100644
--- a/src/main/deploy/elastic-layout.json
+++ b/src/main/deploy/elastic-layout.json
@@ -11,8 +11,8 @@
             "title": "Field",
             "x": 0.0,
             "y": 0.0,
-            "width": 768.0,
-            "height": 384.0,
+            "width": 1024.0,
+            "height": 640.0,
             "type": "Field",
             "properties": {
               "topic": "/SmartDashboard/Field",
@@ -29,8 +29,8 @@
           },
           {
             "title": "Pigeon 2 (v6) [50]",
-            "x": 768.0,
-            "y": 0.0,
+            "x": 1088.0,
+            "y": 128.0,
             "width": 256.0,
             "height": 256.0,
             "type": "Gyro",
@@ -42,8 +42,8 @@
           },
           {
             "title": "BatteryVoltage",
-            "x": 0.0,
-            "y": 384.0,
+            "x": 1024.0,
+            "y": 0.0,
             "width": 384.0,
             "height": 128.0,
             "type": "Voltage View",
@@ -98,6 +98,19 @@
               "robot_color": 4294198070,
               "trajectory_color": 4294967295
             }
+          },
+          {
+            "title": "photonvision_Port_1186_Output_MJPEG_Server",
+            "x": 896.0,
+            "y": 64.0,
+            "width": 576.0,
+            "height": 448.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
+              "period": 0.06,
+              "rotation_turns": 0
+            }
           }
         ]
       }
@@ -2062,7 +2075,7 @@
             }
           },
           {
-            "title": "Blue X (m)",
+            "title": "X (m)",
             "x": 640.0,
             "y": 576.0,
             "width": 128.0,
@@ -2076,7 +2089,7 @@
             }
           },
           {
-            "title": "Blue Y (m)",
+            "title": "Y (m)",
             "x": 768.0,
             "y": 576.0,
             "width": 128.0,
@@ -2090,7 +2103,7 @@
             }
           },
           {
-            "title": "Blue Rot (deg)",
+            "title": "Rot (deg)",
             "x": 896.0,
             "y": 576.0,
             "width": 128.0,
@@ -2104,7 +2117,7 @@
             }
           },
           {
-            "title": "Red X (m)",
+            "title": "Flip X (m)",
             "x": 1152.0,
             "y": 576.0,
             "width": 128.0,
@@ -2118,7 +2131,7 @@
             }
           },
           {
-            "title": "Red Y (m)",
+            "title": "Flip Y (m)",
             "x": 1280.0,
             "y": 576.0,
             "width": 128.0,
@@ -2132,7 +2145,7 @@
             }
           },
           {
-            "title": "Red Rot (deg)",
+            "title": "Flip Rot (deg)",
             "x": 1408.0,
             "y": 576.0,
             "width": 128.0,
@@ -2145,19 +2158,6 @@
               "show_submit_button": false
             }
           },
-          {
-            "title": "Reef Level Selected",
-            "x": 640.0,
-            "y": 256.0,
-            "width": 192.0,
-            "height": 128.0,
-            "type": "Large Text Display",
-            "properties": {
-              "topic": "/SmartDashboard/Driver /Misc/Prepare To Score Selection",
-              "period": 0.06,
-              "data_type": "double"
-            }
-          },
           {
             "title": "BatteryVoltage",
             "x": 512.0,
@@ -2238,8 +2238,8 @@
             "title": "FRONT CAM",
             "x": 832.0,
             "y": 0.0,
-            "width": 448.0,
-            "height": 384.0,
+            "width": 256.0,
+            "height": 256.0,
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1184_Output_MJPEG_Server",
@@ -2254,7 +2254,7 @@
           },
           {
             "title": "Distance to Front Tag",
-            "x": 1280.0,
+            "x": 1088.0,
             "y": 0.0,
             "width": 192.0,
             "height": 128.0,
@@ -2265,6 +2265,50 @@
               "data_type": "double",
               "show_submit_button": false
             }
+          },
+          {
+            "title": "Reef Level Selection",
+            "x": 640.0,
+            "y": 256.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Driver /Misc/Prepare Selection",
+              "period": 0.06,
+              "data_type": "double"
+            }
+          },
+          {
+            "title": "NEW FRONT",
+            "x": 896.0,
+            "y": 256.0,
+            "width": 320.0,
+            "height": 320.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
+              "period": 0.06,
+              "rotation_turns": 0,
+              "resolution": [
+                160.0,
+                120.0
+              ]
+            }
+          },
+          {
+            "title": "Distance to NEW front",
+            "x": 1088.0,
+            "y": 128.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/VisionSubsystem/Distances To Target/left_cam",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
           }
         ]
       }
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 15e858d3..80220abc 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 5ee9f999..5515553c 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -63,25 +63,36 @@ public RobotConfigComp() {
     //                 Units.degreesToRadians(12),
     //                 Units.degreesToRadians(-33),
     //                 Units.degreesToRadians(170)))));
+    // vision.addCamera(
+    //     new Camera(
+    //         "left_cam", // left
+    //         new Transform3d(
+    //             new Translation3d(
+    //                 Units.inchesToMeters(3.250),
+    //                 Units.inchesToMeters(13.592),
+    //                 Units.inchesToMeters(7.201)),
+    //             new Rotation3d(0.0, Units.degreesToRadians(-5.0),
+    // Units.degreesToRadians(90.0)))));
     vision.addCamera(
         new Camera(
             "left_cam", // left
             new Transform3d(
                 new Translation3d(
-                    Units.inchesToMeters(3.250),
-                    Units.inchesToMeters(13.592),
-                    Units.inchesToMeters(7.201)),
-                new Rotation3d(0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(90.0)))));
+                    Units.inchesToMeters(-6.5),
+                    Units.inchesToMeters(0),
+                    Units.inchesToMeters(23.25)),
+                new Rotation3d(0.0, Units.degreesToRadians(15.0), Units.degreesToRadians(5.0)))));
 
-    vision.addCamera(
-        new Camera(
-            "right_cam", // right
-            new Transform3d(
-                new Translation3d(
-                    Units.inchesToMeters(3.250),
-                    Units.inchesToMeters(-13.592),
-                    Units.inchesToMeters(7.201)),
-                new Rotation3d(0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(270.0)))));
+    // vision.addCamera(
+    //     new Camera(
+    //         "right_cam", // right
+    //         new Transform3d(
+    //             new Translation3d(
+    //                 Units.inchesToMeters(3.250),
+    //                 Units.inchesToMeters(-13.592),
+    //                 Units.inchesToMeters(7.201)),
+    //             new Rotation3d(0.0, Units.degreesToRadians(-5.0),
+    // Units.degreesToRadians(270.0)))));
     vision.addCamera(
         new Camera(
             "front_cam", // front
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 672f0394..62d6493c 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -18,14 +18,40 @@ public enum TargetPose {
   REEF_D(6, "D", "Reef D", new Pose2d(4.10, 2.76, Rotation2d.fromDegrees(60)), 1),
   REEF_E(7, "E", "Reef E", new Pose2d(5.00, 2.83, Rotation2d.fromDegrees(120)), -1),
   REEF_F(8, "F", "Reef F", new Pose2d(5.36, 3.00, Rotation2d.fromDegrees(122)), 1),
-  REEF_G(9, "G", "Reef G", new Pose2d(5.80, 3.75, Rotation2d.fromDegrees(183)), -1),
-  REEF_H(10, "H", "Reef H", new Pose2d(5.80, 4.20, Rotation2d.fromDegrees(180)), 1),
+  REEF_G(9, "G", "Reef G", new Pose2d(5.81, 3.90, Rotation2d.fromDegrees(180)), -1),
+  REEF_H(10, "H", "Reef H", new Pose2d(5.87, 4.20, Rotation2d.fromDegrees(180)), 1),
   REEF_I(11, "I", "Reef I", new Pose2d(5.28, 5.06, Rotation2d.fromDegrees(-120)), -1),
   REEF_J(12, "J", "Reef J", new Pose2d(4.97, 5.28, Rotation2d.fromDegrees(-120)), 1),
   REEF_K(13, "K", "Reef K", new Pose2d(3.95, 5.18, Rotation2d.fromDegrees(-60)), -1),
   REEF_L(14, "L", "Reef L", new Pose2d(3.69, 5.04, Rotation2d.fromDegrees(-60)), 1),
   PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0);
 
+  /*
+   *  Home positions (blue)
+   * A:
+   * B:
+   * C: 3.72 3.03 60
+   * D:
+   * E:
+   * F:
+   * G:
+   * H:
+   * I:
+   * J:
+   * K:
+   * L:
+   *
+   *
+   * camera
+   * x -7
+   * y 2
+   * z 22.5in
+   * 23.75
+   * roll 0
+   * pitch 21
+   * yaw 10
+   */
+
   /*
    *  Practice field testing
    * A: 3.13 4.18 0           14.36 3.88 180

From d02bacc8ef36b79b34ba2f671ae0ee7ee9432194 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Tue, 11 Mar 2025 21:27:29 -0400
Subject: [PATCH 62/90] reef pose definitions using math instead of guessing
 (cringe implementation though)

- also fixing temp camera pose to be more realistic
- reduced keep-out zones in pathplanner navgrid
---
 src/main/deploy/pathplanner/navgrid.json      |   2 +-
 .../game/reefscape2025/RobotConfigComp.java   |   8 +-
 .../subsystems/controls/drive/TargetPose.java | 131 ++++++++++++++++--
 3 files changed, 123 insertions(+), 18 deletions(-)

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 80220abc..f827e092 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 5515553c..f8622dfd 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -78,10 +78,10 @@ public RobotConfigComp() {
             "left_cam", // left
             new Transform3d(
                 new Translation3d(
-                    Units.inchesToMeters(-6.5),
-                    Units.inchesToMeters(0),
-                    Units.inchesToMeters(23.25)),
-                new Rotation3d(0.0, Units.degreesToRadians(15.0), Units.degreesToRadians(5.0)))));
+                    Units.inchesToMeters(-7.5),
+                    Units.inchesToMeters(-1),
+                    Units.inchesToMeters(23.35)),
+                new Rotation3d(0.0, Units.degreesToRadians(15), Units.degreesToRadians(5)))));
 
     // vision.addCamera(
     //     new Camera(
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 62d6493c..9fbe8660 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -3,33 +3,133 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
 
 public enum TargetPose {
   // NOTE: WE ARE IDENTIFYING THESE POSITIONS ON THE BLUE SIDE OF THE FIELD!!!
   // THEN, DEPENDING ON THE ALLIANCE GIVEN TO THE DRIVER STATION,
   // PATHPLANNER WILL DECIDE WHETHER THE DESTINATION POSE IS
   // ON THE RED OR BLUE SIDE OF THE FIELD
+
   ORIGIN(0, "O", "Origin", new Pose2d(0.0, 0.0, new Rotation2d(0)), 0),
   FEEDER_R(1, "FR", "Feeder Right", new Pose2d(1.05, 1, new Rotation2d(50)), 0),
   FEEDER_L(2, "FL", "Feeder Left", new Pose2d(1.05, 7, Rotation2d.fromDegrees(-50)), 0),
-  REEF_A(3, "A", "Reef A", new Pose2d(3.14, 4.18, Rotation2d.fromDegrees(0)), -1),
-  REEF_B(4, "B", "Reef B", new Pose2d(3.14, 3.79, Rotation2d.fromDegrees(0)), 1),
-  REEF_C(5, "C", "Reef C", new Pose2d(3.71, 2.98, Rotation2d.fromDegrees(60)), -1),
-  REEF_D(6, "D", "Reef D", new Pose2d(4.10, 2.76, Rotation2d.fromDegrees(60)), 1),
-  REEF_E(7, "E", "Reef E", new Pose2d(5.00, 2.83, Rotation2d.fromDegrees(120)), -1),
-  REEF_F(8, "F", "Reef F", new Pose2d(5.36, 3.00, Rotation2d.fromDegrees(122)), 1),
-  REEF_G(9, "G", "Reef G", new Pose2d(5.81, 3.90, Rotation2d.fromDegrees(180)), -1),
-  REEF_H(10, "H", "Reef H", new Pose2d(5.87, 4.20, Rotation2d.fromDegrees(180)), 1),
-  REEF_I(11, "I", "Reef I", new Pose2d(5.28, 5.06, Rotation2d.fromDegrees(-120)), -1),
-  REEF_J(12, "J", "Reef J", new Pose2d(4.97, 5.28, Rotation2d.fromDegrees(-120)), 1),
-  REEF_K(13, "K", "Reef K", new Pose2d(3.95, 5.18, Rotation2d.fromDegrees(-60)), -1),
-  REEF_L(14, "L", "Reef L", new Pose2d(3.69, 5.04, Rotation2d.fromDegrees(-60)), 1),
+  // REEF_A(3, "A", "Reef A", new Pose2d(3.14, 4.18, Rotation2d.fromDegrees(0)), -1),
+  // REEF_B(4, "B", "Reef B", new Pose2d(3.14, 3.79, Rotation2d.fromDegrees(0)), 1),
+  // REEF_C(5, "C", "Reef C", new Pose2d(3.71, 2.98, Rotation2d.fromDegrees(60)), -1),
+  // // REEF_D(6, "D", "Reef D", new Pose2d(4.10, 2.76, Rotation2d.fromDegrees(60)), 1),
+  // REEF_D(6, "D", "Reef D", rotatePoseAroundReef(new Pose2d(3.16, 3.82,
+  // Rotation2d.fromDegrees(0))), 1),
+  // REEF_E(7, "E", "Reef E", new Pose2d(5.00, 2.83, Rotation2d.fromDegrees(120)), -1),
+  // REEF_F(8, "F", "Reef F", new Pose2d(5.36, 3.00, Rotation2d.fromDegrees(122)), 1),
+  // REEF_G(9, "G", "Reef G", new Pose2d(5.81, 3.90, Rotation2d.fromDegrees(180)), -1),
+  // REEF_H(10, "H", "Reef H", new Pose2d(5.87, 4.20, Rotation2d.fromDegrees(180)), 1),
+  // REEF_I(11, "I", "Reef I", new Pose2d(5.28, 5.06, Rotation2d.fromDegrees(-120)), -1),
+  // REEF_J(12, "J", "Reef J", new Pose2d(4.97, 5.28, Rotation2d.fromDegrees(-120)), 1),
+  // REEF_K(13, "K", "Reef K", new Pose2d(3.95, 5.18, Rotation2d.fromDegrees(-60)), -1),
+  // REEF_L(14, "L", "Reef L", new Pose2d(3.69, 5.04, Rotation2d.fromDegrees(-60)), 1),
+  REEF_A(
+      3,
+      "A",
+      "Reef A",
+      rotatePoseAroundReef(
+          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
+              .transformBy(
+                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
+          0),
+      -1),
+  REEF_B(4, "B", "Reef B", new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), 1),
+  REEF_C(
+      5,
+      "C",
+      "Reef C",
+      rotatePoseAroundReef(
+          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
+              .transformBy(
+                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
+          60),
+      -1),
+  // REEF_D(6, "D", "Reef D", new Pose2d(4.10, 2.76, Rotation2d.fromDegrees(60)), 1),
+  REEF_D(
+      6,
+      "D",
+      "Reef D",
+      rotatePoseAroundReef(new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), 60),
+      1),
+  REEF_E(
+      7,
+      "E",
+      "Reef E",
+      rotatePoseAroundReef(
+          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
+              .transformBy(
+                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
+          120),
+      -1),
+  REEF_F(
+      8,
+      "F",
+      "Reef F",
+      rotatePoseAroundReef(new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), 120),
+      1),
+  REEF_G(
+      9,
+      "G",
+      "Reef G",
+      rotatePoseAroundReef(
+          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
+              .transformBy(
+                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
+          180),
+      -1),
+  REEF_H(
+      10,
+      "H",
+      "Reef H",
+      rotatePoseAroundReef(new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), 180),
+      1),
+  REEF_I(
+      11,
+      "I",
+      "Reef I",
+      rotatePoseAroundReef(
+          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
+              .transformBy(
+                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
+          -120),
+      -1),
+  REEF_J(
+      12,
+      "J",
+      "Reef J",
+      rotatePoseAroundReef(new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), -120),
+      1),
+  REEF_K(
+      13,
+      "K",
+      "Reef K",
+      rotatePoseAroundReef(
+          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
+              .transformBy(
+                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
+          -60),
+      -1),
+  REEF_L(
+      14,
+      "L",
+      "Reef L",
+      rotatePoseAroundReef(new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), -60),
+      1),
   PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0);
 
+  // REMEMBER: actual distance between reef poles is 13 inches,
+  // currently using 9 inches because robot currently overshoots target pose during traversal
+
   /*
    *  Home positions (blue)
    * A:
-   * B:
+   * B: 3.16 3.80
    * C: 3.72 3.03 60
    * D:
    * E:
@@ -136,4 +236,9 @@ private TargetPose(int idx, String sName, String lName, Pose2d targetPose, int r
       this.endPose = targetPose.transformBy(new Transform2d(-0.7, 0, new Rotation2d()));
     }
   }
+
+  private static Pose2d rotatePoseAroundReef(Pose2d pose, double rot) {
+    Pose2d reefCenter = new Pose2d(new Translation2d(4.485, 4.00), new Rotation2d(0));
+    return pose.rotateAround(reefCenter.getTranslation(), Rotation2d.fromDegrees(rot));
+  }
 }

From a711121bd45c07e74fcb316ba73dd5b594afa872 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Wed, 12 Mar 2025 16:13:09 -0400
Subject: [PATCH 63/90] Improved structure for TargetPose definitions

---
 .../subsystems/controls/drive/TargetPose.java | 137 ++++--------------
 1 file changed, 28 insertions(+), 109 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 9fbe8660..50100af3 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -15,117 +15,26 @@ public enum TargetPose {
   ORIGIN(0, "O", "Origin", new Pose2d(0.0, 0.0, new Rotation2d(0)), 0),
   FEEDER_R(1, "FR", "Feeder Right", new Pose2d(1.05, 1, new Rotation2d(50)), 0),
   FEEDER_L(2, "FL", "Feeder Left", new Pose2d(1.05, 7, Rotation2d.fromDegrees(-50)), 0),
-  // REEF_A(3, "A", "Reef A", new Pose2d(3.14, 4.18, Rotation2d.fromDegrees(0)), -1),
-  // REEF_B(4, "B", "Reef B", new Pose2d(3.14, 3.79, Rotation2d.fromDegrees(0)), 1),
-  // REEF_C(5, "C", "Reef C", new Pose2d(3.71, 2.98, Rotation2d.fromDegrees(60)), -1),
-  // // REEF_D(6, "D", "Reef D", new Pose2d(4.10, 2.76, Rotation2d.fromDegrees(60)), 1),
-  // REEF_D(6, "D", "Reef D", rotatePoseAroundReef(new Pose2d(3.16, 3.82,
-  // Rotation2d.fromDegrees(0))), 1),
-  // REEF_E(7, "E", "Reef E", new Pose2d(5.00, 2.83, Rotation2d.fromDegrees(120)), -1),
-  // REEF_F(8, "F", "Reef F", new Pose2d(5.36, 3.00, Rotation2d.fromDegrees(122)), 1),
-  // REEF_G(9, "G", "Reef G", new Pose2d(5.81, 3.90, Rotation2d.fromDegrees(180)), -1),
-  // REEF_H(10, "H", "Reef H", new Pose2d(5.87, 4.20, Rotation2d.fromDegrees(180)), 1),
-  // REEF_I(11, "I", "Reef I", new Pose2d(5.28, 5.06, Rotation2d.fromDegrees(-120)), -1),
-  // REEF_J(12, "J", "Reef J", new Pose2d(4.97, 5.28, Rotation2d.fromDegrees(-120)), 1),
-  // REEF_K(13, "K", "Reef K", new Pose2d(3.95, 5.18, Rotation2d.fromDegrees(-60)), -1),
-  // REEF_L(14, "L", "Reef L", new Pose2d(3.69, 5.04, Rotation2d.fromDegrees(-60)), 1),
-  REEF_A(
-      3,
-      "A",
-      "Reef A",
-      rotatePoseAroundReef(
-          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
-              .transformBy(
-                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
-          0),
-      -1),
-  REEF_B(4, "B", "Reef B", new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), 1),
-  REEF_C(
-      5,
-      "C",
-      "Reef C",
-      rotatePoseAroundReef(
-          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
-              .transformBy(
-                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
-          60),
-      -1),
-  // REEF_D(6, "D", "Reef D", new Pose2d(4.10, 2.76, Rotation2d.fromDegrees(60)), 1),
-  REEF_D(
-      6,
-      "D",
-      "Reef D",
-      rotatePoseAroundReef(new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), 60),
-      1),
-  REEF_E(
-      7,
-      "E",
-      "Reef E",
-      rotatePoseAroundReef(
-          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
-              .transformBy(
-                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
-          120),
-      -1),
-  REEF_F(
-      8,
-      "F",
-      "Reef F",
-      rotatePoseAroundReef(new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), 120),
-      1),
-  REEF_G(
-      9,
-      "G",
-      "Reef G",
-      rotatePoseAroundReef(
-          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
-              .transformBy(
-                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
-          180),
-      -1),
-  REEF_H(
-      10,
-      "H",
-      "Reef H",
-      rotatePoseAroundReef(new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), 180),
-      1),
-  REEF_I(
-      11,
-      "I",
-      "Reef I",
-      rotatePoseAroundReef(
-          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
-              .transformBy(
-                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
-          -120),
-      -1),
-  REEF_J(
-      12,
-      "J",
-      "Reef J",
-      rotatePoseAroundReef(new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), -120),
-      1),
-  REEF_K(
-      13,
-      "K",
-      "Reef K",
-      rotatePoseAroundReef(
-          new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0))
-              .transformBy(
-                  new Transform2d(new Translation2d(0, Units.inchesToMeters(9)), new Rotation2d())),
-          -60),
-      -1),
-  REEF_L(
-      14,
-      "L",
-      "Reef L",
-      rotatePoseAroundReef(new Pose2d(3.16, 3.82, Rotation2d.fromDegrees(0)), -60),
-      1),
+  REEF_A(3, "A", "Reef A", 0.0, true),
+  REEF_B(4, "B", "Reef B", 0.0, false),
+  REEF_C(5, "C", "Reef C", 60.0, true),
+  REEF_D(6, "D", "Reef D", 60.0, false),
+  REEF_E(7, "E", "Reef E", 120.0, true),
+  REEF_F(8, "F", "Reef F", 120.0, false),
+  REEF_G(9, "G", "Reef G", 180.0, true),
+  REEF_H(10, "H", "Reef H", 180.0, false),
+  REEF_I(11, "I", "Reef I", 240.0, true),
+  REEF_J(12, "J", "Reef J", 240.0, false),
+  REEF_K(13, "K", "Reef K", 300.0, true),
+  REEF_L(14, "L", "Reef L", 300.0, false),
   PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0);
 
   // REMEMBER: actual distance between reef poles is 13 inches,
   // currently using 9 inches because robot currently overshoots target pose during traversal
 
+  private final Pose2d blueReefCenter = new Pose2d(new Translation2d(4.485, 4.00), new Rotation2d());
+  private final Pose2d blueReefBPos = new Pose2d(new Translation2d(3.16, 3.82), new Rotation2d());
+
   /*
    *  Home positions (blue)
    * A:
@@ -237,8 +146,18 @@ private TargetPose(int idx, String sName, String lName, Pose2d targetPose, int r
     }
   }
 
-  private static Pose2d rotatePoseAroundReef(Pose2d pose, double rot) {
-    Pose2d reefCenter = new Pose2d(new Translation2d(4.485, 4.00), new Rotation2d(0));
-    return pose.rotateAround(reefCenter.getTranslation(), Rotation2d.fromDegrees(rot));
+  private TargetPose(int idx, String sName, String lName, double reefWallRotDegrees, boolean isLeftPosition) {
+    this.index = idx;
+    this.shortName = sName;
+    this.longName = lName;
+    
+    this.pose = blueReefBPos.rotateAround(blueReefCenter.getTranslation(), Rotation2d.fromDegrees(reefWallRotDegrees));
+    double distance = 0.7;
+    if (isLeftPosition) {
+      this.pose = this.pose.transformBy(new Transform2d(0, Units.inchesToMeters(9), new Rotation2d()));
+      distance = -distance;
+    }
+    this.prepPose = this.pose.transformBy(new Transform2d(0, distance, new Rotation2d()));
+    this.endPose = this.pose.transformBy(new Transform2d(-0.7, 0, new Rotation2d()));
   }
 }

From 7034b77f4452754a80791a6842b805c3773b1aed Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Wed, 12 Mar 2025 16:46:09 -0400
Subject: [PATCH 64/90] Added the two cameras in front. Have not renamed them.
 Also ran spotlessApply

---
 .../game/reefscape2025/RobotConfigComp.java      | 16 ++++++++--------
 .../subsystems/controls/drive/TargetPose.java    | 15 ++++++++++-----
 2 files changed, 18 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index f8622dfd..06bc20f4 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -78,10 +78,10 @@ public RobotConfigComp() {
             "left_cam", // left
             new Transform3d(
                 new Translation3d(
-                    Units.inchesToMeters(-7.5),
-                    Units.inchesToMeters(-1),
-                    Units.inchesToMeters(23.35)),
-                new Rotation3d(0.0, Units.degreesToRadians(15), Units.degreesToRadians(5)))));
+                    Units.inchesToMeters(9.343),
+                    Units.inchesToMeters(11.974),
+                    Units.inchesToMeters(11.110)),
+                new Rotation3d(0.0, Units.degreesToRadians(5.0), Units.degreesToRadians(-22)))));
 
     // vision.addCamera(
     //     new Camera(
@@ -98,10 +98,10 @@ public RobotConfigComp() {
             "front_cam", // front
             new Transform3d(
                 new Translation3d(
-                    Units.inchesToMeters(13.592),
-                    Units.inchesToMeters(2.75),
-                    Units.inchesToMeters(7.201)),
-                new Rotation3d(0.0, Units.degreesToRadians(-5.0), 0.0))));
+                    Units.inchesToMeters(9.343),
+                    Units.inchesToMeters(-11.974),
+                    Units.inchesToMeters(11.110)),
+                new Rotation3d(0.0, Units.degreesToRadians(5.0), Units.degreesToRadians(22)))));
 
     // Elevator
     {
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 50100af3..766f8c45 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -32,7 +32,8 @@ public enum TargetPose {
   // REMEMBER: actual distance between reef poles is 13 inches,
   // currently using 9 inches because robot currently overshoots target pose during traversal
 
-  private final Pose2d blueReefCenter = new Pose2d(new Translation2d(4.485, 4.00), new Rotation2d());
+  private final Pose2d blueReefCenter =
+      new Pose2d(new Translation2d(4.485, 4.00), new Rotation2d());
   private final Pose2d blueReefBPos = new Pose2d(new Translation2d(3.16, 3.82), new Rotation2d());
 
   /*
@@ -146,15 +147,19 @@ private TargetPose(int idx, String sName, String lName, Pose2d targetPose, int r
     }
   }
 
-  private TargetPose(int idx, String sName, String lName, double reefWallRotDegrees, boolean isLeftPosition) {
+  private TargetPose(
+      int idx, String sName, String lName, double reefWallRotDegrees, boolean isLeftPosition) {
     this.index = idx;
     this.shortName = sName;
     this.longName = lName;
-    
-    this.pose = blueReefBPos.rotateAround(blueReefCenter.getTranslation(), Rotation2d.fromDegrees(reefWallRotDegrees));
+
+    this.pose =
+        blueReefBPos.rotateAround(
+            blueReefCenter.getTranslation(), Rotation2d.fromDegrees(reefWallRotDegrees));
     double distance = 0.7;
     if (isLeftPosition) {
-      this.pose = this.pose.transformBy(new Transform2d(0, Units.inchesToMeters(9), new Rotation2d()));
+      this.pose =
+          this.pose.transformBy(new Transform2d(0, Units.inchesToMeters(9), new Rotation2d()));
       distance = -distance;
     }
     this.prepPose = this.pose.transformBy(new Transform2d(0, distance, new Rotation2d()));

From 6e980ebfa2cbb29668a38ec700538bd05641ed04 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Wed, 12 Mar 2025 17:59:12 -0400
Subject: [PATCH 65/90] Modified prep score path for better camera view of the
 reef april tag

---
 .../robot/subsystems/controls/drive/TargetPose.java | 13 +++++++++----
 1 file changed, 9 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 766f8c45..d21511d9 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -34,7 +34,10 @@ public enum TargetPose {
 
   private final Pose2d blueReefCenter =
       new Pose2d(new Translation2d(4.485, 4.00), new Rotation2d());
-  private final Pose2d blueReefBPos = new Pose2d(new Translation2d(3.16, 3.82), new Rotation2d());
+
+  // NOTE: I pushed the target position closer to the reef to be more realistic to the actual game
+  // the original position was x: 3.16, y: 3.82
+  private final Pose2d blueReefBPos = new Pose2d(new Translation2d(3.19, 3.82), new Rotation2d());
 
   /*
    *  Home positions (blue)
@@ -156,13 +159,15 @@ private TargetPose(
     this.pose =
         blueReefBPos.rotateAround(
             blueReefCenter.getTranslation(), Rotation2d.fromDegrees(reefWallRotDegrees));
-    double distance = 0.7;
+    double distance = 0.6;
+    double rot = -Units.degreesToRadians(25.0);
     if (isLeftPosition) {
       this.pose =
-          this.pose.transformBy(new Transform2d(0, Units.inchesToMeters(9), new Rotation2d()));
+          this.pose.transformBy(new Transform2d(0, Units.inchesToMeters(13), new Rotation2d()));
       distance = -distance;
+      rot = -rot;
     }
-    this.prepPose = this.pose.transformBy(new Transform2d(0, distance, new Rotation2d()));
+    this.prepPose = this.pose.transformBy(new Transform2d(-0.2, distance, new Rotation2d(rot)));
     this.endPose = this.pose.transformBy(new Transform2d(-0.7, 0, new Rotation2d()));
   }
 }

From 31c0bfd461a870cf1f0d17499a7570bff2dce644 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Wed, 12 Mar 2025 19:08:54 -0400
Subject: [PATCH 66/90] tweaks

---
 .../java/frc/robot/subsystems/controls/drive/TargetPose.java    | 2 +-
 .../subsystems/implementations/vision/VisionSubsystem.java      | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index d21511d9..72a5e7f2 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -37,7 +37,7 @@ public enum TargetPose {
 
   // NOTE: I pushed the target position closer to the reef to be more realistic to the actual game
   // the original position was x: 3.16, y: 3.82
-  private final Pose2d blueReefBPos = new Pose2d(new Translation2d(3.19, 3.82), new Rotation2d());
+  private final Pose2d blueReefBPos = new Pose2d(new Translation2d(3.175, 3.82), new Rotation2d());
 
   /*
    *  Home positions (blue)
diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index 2342f982..e0c047f9 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -166,7 +166,7 @@ public void updatePoseEstimation() {
         distanceToTarget = camera.getDistanceToBestTarget();
 
         // Add vision measurement to the consumer.
-        if (visionMeasurementConsumer != null && distanceToTarget < 2) {
+        if (visionMeasurementConsumer != null && distanceToTarget < 0.8) {
           visionMeasurementConsumer.add(
               estimatedRobotPoseFromCamera,
               currentEstimatedRobotPose.get().timestampSeconds,

From f2cd9865715f6a3498e6a27a0adfdb370ac3fada Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Wed, 12 Mar 2025 20:32:10 -0400
Subject: [PATCH 67/90] Added reef Chooser Debug code

---
 simulation.properties                              |  2 +-
 .../subsystems/controls/drive/DriveControls.java   | 14 ++++++++++++++
 .../subsystems/controls/drive/TargetPose.java      |  9 +++++++++
 3 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/simulation.properties b/simulation.properties
index 2d763eed..b731d52c 100644
--- a/simulation.properties
+++ b/simulation.properties
@@ -1 +1 @@
-robot.name=STUB
\ No newline at end of file
+robot.name=NEMO
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index c9e1592e..a77c5c84 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -2,9 +2,11 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -24,6 +26,7 @@
 import frc.robot.subsystems.interfaces.Elevator;
 import java.util.Map;
 import java.util.Map.Entry;
+import org.littletonrobotics.junction.Logger;
 
 public class DriveControls {
 
@@ -256,6 +259,16 @@ public static void setupAssistantController(Drive drive, CommandXboxController c
                     myCoolPoseKeyIdx -= 12;
                   }
                   SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
+                  Pose2d ReefPose;
+                  if (DriverStation.Alliance.Red == DriverStation.getAlliance().get()) {
+                    ReefPose =
+                        FlippingUtil.flipFieldPose(TargetPose.getPosewWithIndex(myCoolPoseKeyIdx));
+                  } else {
+                    ReefPose = TargetPose.getPosewWithIndex(myCoolPoseKeyIdx);
+                  }
+                  Logger.recordOutput("Chosen Reef Pose", ReefPose);
+                  SmartDashboard.putNumber("Chosen Reef X", ReefPose.getX());
+                  SmartDashboard.putNumber("Chosen Reef Y", ReefPose.getY());
                 }));
   }
 
@@ -274,6 +287,7 @@ private static Entry<Integer, Command> coolDynamicPathScoringCommand(
                 prepareToScoreCommand,
                 AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
                 scoreCommand));
+
     return entry;
   }
 
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 72a5e7f2..4e3b41f9 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -123,6 +123,15 @@ public Pose2d getPose() {
     return this.pose;
   }
 
+  public static Pose2d getPosewWithIndex(int index) {
+    for (TargetPose x : values()) {
+      if (x.getIndex() == index) {
+        return x.pose;
+      }
+    }
+    return new Pose2d();
+  }
+
   public Pose2d getPrepPose() {
     return this.prepPose;
   }

From 74d8ad1fcebd4d11ff76558eb1b02953dfaf6d76 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Wed, 12 Mar 2025 20:54:57 -0400
Subject: [PATCH 68/90] l2 l3 diff score

---
 .../controls/combination/DriverAssistControls.java       | 8 +++++++-
 .../robot/subsystems/controls/drive/DriveControls.java   | 9 ++++++++-
 .../frc/robot/subsystems/controls/drive/TargetPose.java  | 6 ++++++
 3 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
index 6b9ce176..358fe03b 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
@@ -8,8 +8,11 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.elevator.ElevatorToPosition;
+import frc.robot.commands.common.motor.MotorAutoResetEncoderCommand;
+import frc.robot.subsystems.controls.arm.CoralArmControls;
 import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Elevator;
+import frc.robot.subsystems.interfaces.Motor;
 import frc.robot.subsystems.interfaces.SimpleMotor;
 
 public class DriverAssistControls {
@@ -59,7 +62,10 @@ public static void setupController(
                       SmartDashboard.putNumber(
                           "Driver " + "/Misc/Prepare Selection",
                           DriverControls.Constants.prepareScoreSelctedIndex);
-                    })));
+                    }))
+            .andThen(
+                new MotorAutoResetEncoderCommand(
+                    (Motor) coralArm, CoralArmControls.Constants.autoZeroSettings)));
 
     controller
         .y()
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index c9e1592e..81ac9c1b 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -272,7 +272,14 @@ private static Entry<Integer, Command> coolDynamicPathScoringCommand(
             new SequentialCommandGroup(
                 AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
                 prepareToScoreCommand,
-                AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
+                new SelectCommand<>(
+                    Map.ofEntries(
+                        Map.entry(0, AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0)),
+                        Map.entry(1, AutoBuilder.pathfindToPoseFlipped(target.getLowerScoringPose(), constraints, 0.0))),
+                        () -> {
+                            return DriverControls.Constants.prepareScoreSelctedIndex < 4 ? 1 : 0;
+                          }
+                ),
                 scoreCommand));
     return entry;
   }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 72a5e7f2..188a9c35 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -103,6 +103,7 @@ public enum TargetPose {
 
   private Pose2d pose;
   private Pose2d prepPose;
+  private Pose2d lowerLevelPose;
   private Pose2d endPose;
 
   // NOTE differential poses ~= 0.5m away from target
@@ -131,6 +132,9 @@ public Pose2d getEndPose(boolean isRed) {
     return this.endPose;
   }
 
+  public Pose2d getLowerScoringPose() {
+    return this.lowerLevelPose;
+  }
   private TargetPose(int idx, String sName, String lName, Pose2d targetPose, int reefPosition) {
     this.index = idx;
     this.shortName = sName;
@@ -146,6 +150,7 @@ private TargetPose(int idx, String sName, String lName, Pose2d targetPose, int r
     } else {
       double distance = (reefPosition < 0) ? -0.7 : 0.7;
       this.prepPose = targetPose.transformBy(new Transform2d(0, distance, new Rotation2d()));
+      this.lowerLevelPose = this.pose.transformBy(new Transform2d(0.04445, 0, new Rotation2d()));
       this.endPose = targetPose.transformBy(new Transform2d(-0.7, 0, new Rotation2d()));
     }
   }
@@ -168,6 +173,7 @@ private TargetPose(
       rot = -rot;
     }
     this.prepPose = this.pose.transformBy(new Transform2d(-0.2, distance, new Rotation2d(rot)));
+    this.lowerLevelPose = this.pose.transformBy(new Transform2d(0.04445, 0, new Rotation2d()));
     this.endPose = this.pose.transformBy(new Transform2d(-0.7, 0, new Rotation2d()));
   }
 }

From 92f7d5f04466ecefac48355a166bc16c34e36470 Mon Sep 17 00:00:00 2001
From: danielcastellarin <daniel.r.castellarin@gmail.com>
Date: Thu, 13 Mar 2025 14:28:34 -0400
Subject: [PATCH 69/90] Code cleanup

New Elastic dashboard for drivers
Cleaned up network table entries
TargetPose enum is stored for user selection instead of an index
Fixed camera distance to AprilTag estimation
---
 src/main/deploy/elastic-layout.json           | 360 +++++++++++++++---
 .../combination/DriverAssistControls.java     |  33 +-
 .../controls/combination/DriverControls.java  |  28 +-
 .../controls/drive/DriveControls.java         | 140 ++-----
 .../subsystems/controls/drive/TargetPose.java |  33 +-
 .../vision/VisionSubsystem.java               |   2 +-
 6 files changed, 387 insertions(+), 209 deletions(-)

diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json
index f1043190..b5ac3a91 100644
--- a/src/main/deploy/elastic-layout.json
+++ b/src/main/deploy/elastic-layout.json
@@ -1,6 +1,6 @@
 {
   "version": 1.0,
-  "grid_size": 64,
+  "grid_size": 32,
   "tabs": [
     {
       "name": "Teleoperated",
@@ -130,18 +130,6 @@
               "label_position": "TOP"
             },
             "children": []
-          },
-          {
-            "title": "Reef Location",
-            "x": 576.0,
-            "y": 256.0,
-            "width": 512.0,
-            "height": 128.0,
-            "type": "List Layout",
-            "properties": {
-              "label_position": "TOP"
-            },
-            "children": []
           }
         ],
         "containers": [
@@ -273,6 +261,238 @@
         ]
       }
     },
+    {
+      "name": "NEW Driver",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "Field",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 768.0,
+            "height": 384.0,
+            "type": "Field",
+            "properties": {
+              "topic": "/SmartDashboard/Field",
+              "period": 0.06,
+              "field_game": "Reefscape",
+              "robot_width": 0.85,
+              "robot_length": 0.85,
+              "show_other_objects": true,
+              "show_trajectories": true,
+              "field_rotation": 0.0,
+              "robot_color": 4294198070,
+              "trajectory_color": 4294967295
+            }
+          },
+          {
+            "title": "Auto Calibrate Elevator",
+            "x": 256.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Elevator[Coral]/Commands/Auto Calibrate Elevator",
+              "period": 0.06,
+              "show_type": true
+            }
+          },
+          {
+            "title": "Auto Calibrate Coral Arm",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Arm[Coral]/Commands/Auto Calibrate Coral Arm",
+              "period": 0.06,
+              "show_type": true
+            }
+          },
+          {
+            "title": "Arm Angle (deg)",
+            "x": 0.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Arm[Coral]/PositionDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Elevetor Position (m)",
+            "x": 256.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Elevator[Coral]/PositionMeters",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Alliance Color",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/FMSInfo/IsRedAlliance",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4294901760,
+              "false_color": 4278190335,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Is Field Orient",
+            "x": 768.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/DriveSwerveYAGSL/FieldOrientedDrive",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "RIGHT CAM",
+            "x": 1152.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/photonvision_Port_1184_Output_MJPEG_Server",
+              "period": 0.06,
+              "rotation_turns": 0,
+              "fps": 10,
+              "resolution": [
+                160.0,
+                120.0
+              ]
+            }
+          },
+          {
+            "title": "Distance to Right Tag",
+            "x": 1152.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/VisionSubsystem/Distances To Target/front_cam",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "LEFT CAM",
+            "x": 896.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
+              "period": 0.06,
+              "rotation_turns": 0,
+              "resolution": [
+                160.0,
+                120.0
+              ]
+            }
+          },
+          {
+            "title": "Distance to Left Tag",
+            "x": 896.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/VisionSubsystem/Distances To Target/left_cam",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Reef Level",
+            "x": 768.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Driver /Misc/Prepare Selection",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Chosen Reef",
+            "x": 768.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Chosen Reef Position",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Auto Calibrate Climber",
+            "x": 512.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Arm[Climber]/Commands/Climber Auto Calibrate ",
+              "period": 0.06,
+              "show_type": true
+            }
+          },
+          {
+            "title": "Climber Angle (deg)",
+            "x": 512.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Simple[Climber]/PositionDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    },
     {
       "name": "Pit",
       "grid_layout": {
@@ -2007,19 +2227,6 @@
               "trajectory_color": 4294967295
             }
           },
-          {
-            "title": "Reef Position",
-            "x": 640.0,
-            "y": 128.0,
-            "width": 192.0,
-            "height": 128.0,
-            "type": "Large Text Display",
-            "properties": {
-              "topic": "/SmartDashboard/AAAAAA Reef Position",
-              "period": 0.06,
-              "data_type": "string"
-            }
-          },
           {
             "title": "Auto Calibrate Elevator",
             "x": 256.0,
@@ -2160,7 +2367,7 @@
           },
           {
             "title": "BatteryVoltage",
-            "x": 512.0,
+            "x": 1152.0,
             "y": 448.0,
             "width": 384.0,
             "height": 128.0,
@@ -2235,8 +2442,8 @@
             }
           },
           {
-            "title": "FRONT CAM",
-            "x": 832.0,
+            "title": "RIGHT CAM",
+            "x": 1088.0,
             "y": 0.0,
             "width": 256.0,
             "height": 256.0,
@@ -2253,10 +2460,10 @@
             }
           },
           {
-            "title": "Distance to Front Tag",
+            "title": "Distance to Right Tag",
             "x": 1088.0,
-            "y": 0.0,
-            "width": 192.0,
+            "y": 256.0,
+            "width": 256.0,
             "height": 128.0,
             "type": "Text Display",
             "properties": {
@@ -2266,6 +2473,37 @@
               "show_submit_button": false
             }
           },
+          {
+            "title": "LEFT CAM",
+            "x": 832.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
+              "period": 0.06,
+              "rotation_turns": 0,
+              "resolution": [
+                160.0,
+                120.0
+              ]
+            }
+          },
+          {
+            "title": "Distance to Left Tag",
+            "x": 832.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/VisionSubsystem/Distances To Target/left_cam",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
           {
             "title": "Reef Level Selection",
             "x": 640.0,
@@ -2276,35 +2514,59 @@
             "properties": {
               "topic": "/SmartDashboard/Driver /Misc/Prepare Selection",
               "period": 0.06,
-              "data_type": "double"
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Chosen Reef Position",
+            "x": 640.0,
+            "y": 128.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Chosen Reef Position",
+              "period": 0.06,
+              "data_type": "string"
             }
           },
           {
-            "title": "NEW FRONT",
+            "title": "Reef Rot (deg)",
             "x": 896.0,
-            "y": 256.0,
-            "width": 320.0,
-            "height": 320.0,
-            "type": "Camera Stream",
+            "y": 448.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
             "properties": {
-              "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
+              "topic": "/SmartDashboard/Chosen Reef Rot",
               "period": 0.06,
-              "rotation_turns": 0,
-              "resolution": [
-                160.0,
-                120.0
-              ]
+              "data_type": "double",
+              "show_submit_button": false
             }
           },
           {
-            "title": "Distance to NEW front",
-            "x": 1088.0,
-            "y": 128.0,
-            "width": 192.0,
+            "title": "Reef X (m)",
+            "x": 640.0,
+            "y": 448.0,
+            "width": 128.0,
             "height": 128.0,
             "type": "Text Display",
             "properties": {
-              "topic": "/AdvantageKit/RealOutputs/VisionSubsystem/Distances To Target/left_cam",
+              "topic": "/SmartDashboard/Chosen Reef X",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Reef Y (m)",
+            "x": 768.0,
+            "y": 448.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Chosen Reef Y",
               "period": 0.06,
               "data_type": "double",
               "show_submit_button": false
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
index 358fe03b..738a8e3f 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
@@ -55,17 +55,16 @@ public static void setupController(
     controller
         .b()
         .onTrue(
-            prepareIntakeCoralCommand.andThen(
-                new InstantCommand(
-                    () -> {
-                      DriverControls.Constants.prepareScoreSelctedIndex = 1;
-                      SmartDashboard.putNumber(
-                          "Driver " + "/Misc/Prepare Selection",
-                          DriverControls.Constants.prepareScoreSelctedIndex);
-                    }))
-            .andThen(
-                new MotorAutoResetEncoderCommand(
-                    (Motor) coralArm, CoralArmControls.Constants.autoZeroSettings)));
+            prepareIntakeCoralCommand
+                .andThen(
+                    new InstantCommand(
+                        () -> {
+                          DriverControls.Constants.prepareScoreSelctedIndex = 1;
+                          SmartDashboard.putString("Driver /Misc/Prepare Selection", "N/A");
+                        }))
+                .andThen(
+                    new MotorAutoResetEncoderCommand(
+                        (Motor) coralArm, CoralArmControls.Constants.autoZeroSettings)));
 
     controller
         .y()
@@ -73,9 +72,7 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 4;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L4");
                 }));
 
     controller
@@ -84,9 +81,7 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 3;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L3");
                 }));
 
     controller
@@ -95,9 +90,7 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 2;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L2");
                 }));
 
     // controller
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 2d46cd7d..9c8f9ebf 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -55,9 +55,7 @@ public static void setupController(
                     new InstantCommand(
                         () -> {
                           DriverControls.Constants.prepareScoreSelctedIndex = 1;
-                          SmartDashboard.putNumber(
-                              "Driver " + "/Misc/Prepare Selection",
-                              DriverControls.Constants.prepareScoreSelctedIndex);
+                          SmartDashboard.putString("Driver /Misc/Prepare Selection", "N/A");
                         })));
 
     Command intakeCoralCommand = new ElevatorToPosition(elevator, () -> 0.345);
@@ -81,9 +79,7 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 4;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L4");
                 }));
 
     controller
@@ -92,9 +88,7 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 3;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L3");
                 }));
 
     controller
@@ -103,26 +97,26 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 2;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L2");
                 }));
 
     DriverControls.Constants.prepareChooser.onChange(
         (index) -> {
           DriverControls.Constants.prepareScoreSelctedIndex = index;
-          SmartDashboard.putNumber("Driver " + "/Misc/Prepare To Score Selection", index);
+          String desc;
+          if (index == 2) desc = "L2";
+          else if (index == 3) desc = "L3";
+          else desc = "L4";
+          SmartDashboard.putString("Driver /Misc/Prepare Selection", desc);
         });
 
-    SmartDashboard.putNumber(
-        "Driver " + "/Misc/Prepare To Score Selection", Constants.prepareChooser.getSelected());
+    SmartDashboard.putData("Driver " + "/Misc/Prepare To Score Chooser", Constants.prepareChooser);
+    SmartDashboard.putString("Driver /Misc/Prepare Selection", "L2");
 
     SmartDashboard.putData(
         "Driver " + "/Commands/Prepare To Score Command",
         getPrepareToScoreCommand(elevator, coralArm));
 
-    SmartDashboard.putData("Driver " + "/Misc/Prepare To Score Chooser", Constants.prepareChooser);
-
     // climber
     SubsystemBase climberSubsystem = (SubsystemBase) climber;
     // prepare to climb
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 7fd0f4e9..af27c302 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -5,11 +5,10 @@
 import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -24,19 +23,14 @@
 import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Drive;
 import frc.robot.subsystems.interfaces.Elevator;
+import frc.robot.util.DevilBotState;
 import java.util.Map;
 import java.util.Map.Entry;
 import org.littletonrobotics.junction.Logger;
 
 public class DriveControls {
 
-  protected static int myCoolPoseKeyIdx = TargetPose.REEF_A.getIndex();
-
-  protected static int coolestNumberEver = 0;
-
-  protected static final String[] orderedReefPositions = {
-    "E", "F", "G", "H", "I", "J", "K", "L", "A", "B", "C", "D"
-  };
+  protected static TargetPose chosenTarget = TargetPose.REEF_A;
 
   public static void setupController(
       Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
@@ -61,28 +55,6 @@ public static void setupController(
                     drive.setFieldOrientedDrive(
                         !drive.isFieldOrientedDrive()))); // Toggle Drive Orientation
 
-    // BLUE
-    // // Define destinations for our "dynamic go-to-pose" functionality
-    // Pose2d poseOrigin = new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-    //     poseFeeder1 = new Pose2d(1.05, 7, Rotation2d.fromDegrees(130)),
-    //     poseFeeder2 = new Pose2d(1.05, 1, Rotation2d.fromDegrees(230)),
-    //     poseProcessor = new Pose2d(6, 0.75, Rotation2d.fromDegrees(270)),
-    //     poseReefA = new Pose2d(3.25, 4.05, Rotation2d.fromDegrees(0)),
-    //     poseReefG = new Pose2d(5.5, 3.95, Rotation2d.fromDegrees(180));
-
-    // Define destinations for our "dynamic go-to-pose" functionality
-    Pose2d poseOrigin = new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-        poseFeeder1 = new Pose2d(16.44, 7.25, Rotation2d.fromDegrees(230)),
-        poseFeeder2 = new Pose2d(16.02, 1, Rotation2d.fromDegrees(-230)),
-        poseProcessor = new Pose2d(11.5, 7.3, Rotation2d.fromDegrees(90)),
-        poseReefA = new Pose2d(15, 4.175, Rotation2d.fromDegrees(180)),
-        poseReefAClose = new Pose2d(14.425, 4.175, Rotation2d.fromDegrees(180)),
-        poseReefG = new Pose2d(11.57, 4.17, Rotation2d.fromDegrees(0)),
-        poseReefGClose = new Pose2d(11.5, 4.175, Rotation2d.fromDegrees(0));
-    //  PathConstraints constraints = new PathConstraints(4.9672, 9.3664784, 2 * Math.PI, 4 *
-    //  Math.PI);
-    // PathConstraints constraints = new PathConstraints(2, 1.5, 2 * Math.PI, 4 * Math.PI);
-    // PathConstraints constraints = new PathConstraints(0.5, 4.5, Math.PI / 4, 4 * Math.PI);
     PathConstraints constraints =
         new PathConstraints(
             drive.getMaxLinearSpeed(), 1.5, drive.getMaxAngularSpeed(), Math.PI / 4);
@@ -109,21 +81,14 @@ public static void setupController(
     SmartDashboard.putData("Pose choices", chooser);
 
     // Define behavior for chosing destination of on-the-fly pose
-    SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
+    SmartDashboard.putNumber("Chosen Pose Index", chosenTarget.getIndex());
+    SmartDashboard.putString("Chosen Reef Position", chosenTarget.getShortName());
     chooser.onChange(
-        (chosenPose) -> {
-          myCoolPoseKeyIdx = chosenPose.getIndex();
-          SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
+        (chosenOption) -> {
+          chosenTarget = chosenOption;
+          SmartDashboard.putNumber("Chosen Pose Index", chosenTarget.getIndex());
+          SmartDashboard.putString("Reef Position", chosenTarget.getShortName());
         });
-    // controller
-    //     .x()
-    //     .onTrue(
-    //         new InstantCommand(
-    //             () -> {
-    //               if (++myCoolPoseKeyIdx == TargetPoseOption.values().length) myCoolPoseKeyIdx =
-    // 1;
-    //               SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
-    //             }));
 
     // Define command to go to specific pose
     Command coolGoToPose =
@@ -190,30 +155,12 @@ public static void setupController(
                     getPrepareToScoreCommand(elevator, arm),
                     new ArmToPosition(arm, () -> 0))),
             () -> {
-              return myCoolPoseKeyIdx;
+              return chosenTarget.getIndex();
             });
 
-    // TODO: new dynamic path planning command creation
-    // 1. SelectCommand ONE: go to initial location (based on user-chosen option X)
-    // 2. SelectCommand TWO: determine next action (based on option X and potentially pre-decided
-    // reef elevation value)
-    // 2a. if (1) was a reef position, assume we are scoring --> get reef elevation to score on -->
-    // set elevator/arm
-    // 2b. if not, choose an empty command and short-circuit (exit) the sequence
-    // 3. SelectCommand THREE: go to scoring position (which maps directly from initial chosen
-    // location)
-    // 4. Execute <arm down command>
-
     // dynamically go to destination
     controller.rightTrigger().whileTrue(coolGoToPose);
 
-    // TEMP FUNCTION TO TEST FIELD FLIPPING
-    // controller.b().whileTrue(new SequentialCommandGroup(
-    //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPrepPose(), constraints, 0.0),
-    //     // DriverControls.Constants.prepareScoreCommand,
-    //     AutoBuilder.pathfindToPoseFlipped(TargetPose.REEF_A.getMyPose(), constraints, 0.0),
-    //     new ArmToPosition(arm, () -> 0)));
-
     /*  Angles -> Reef positions
      * 0    30  : E
      * 30   60  : F
@@ -243,32 +190,20 @@ public static void setupAssistantController(Drive drive, CommandXboxController c
                   double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
                   if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
 
-                  // This number can be used to index into the ordered reef positions array!
-                  coolestNumberEver = (int) myNumber / 30;
+                  // Convert the joystick angle to a reef position
+                  chosenTarget = TargetPose.getReefTargetWithAngle(myNumber);
+                  SmartDashboard.putNumber("Chosen Pose Index", chosenTarget.getIndex());
+                  SmartDashboard.putString("Chosen Reef Position", chosenTarget.getShortName());
 
-                  // TODO remove Smartdashboard number; only display reef position
-                  SmartDashboard.putNumber("AAAAAA", coolestNumberEver);
-                  SmartDashboard.putString(
-                      "AAAAAA Reef Position", orderedReefPositions[coolestNumberEver]);
-                }))
-        .onFalse(
-            new InstantCommand(
-                () -> {
-                  myCoolPoseKeyIdx = coolestNumberEver + 7;
-                  if (myCoolPoseKeyIdx > 14) {
-                    myCoolPoseKeyIdx -= 12;
-                  }
-                  SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
-                  Pose2d ReefPose;
-                  if (DriverStation.Alliance.Red == DriverStation.getAlliance().get()) {
-                    ReefPose =
-                        FlippingUtil.flipFieldPose(TargetPose.getPosewWithIndex(myCoolPoseKeyIdx));
-                  } else {
-                    ReefPose = TargetPose.getPosewWithIndex(myCoolPoseKeyIdx);
-                  }
-                  Logger.recordOutput("Chosen Reef Pose", ReefPose);
-                  SmartDashboard.putNumber("Chosen Reef X", ReefPose.getX());
-                  SmartDashboard.putNumber("Chosen Reef Y", ReefPose.getY());
+                  // Log target pose (for debugging)
+                  Pose2d p =
+                      DevilBotState.isRedAlliance()
+                          ? chosenTarget.getPose()
+                          : FlippingUtil.flipFieldPose(chosenTarget.getPose());
+                  Logger.recordOutput("DriveSwerveYAGSL/Chosen Reef Pose", p);
+                  SmartDashboard.putNumber("Chosen Reef X", p.getX());
+                  SmartDashboard.putNumber("Chosen Reef Y", p.getY());
+                  SmartDashboard.putNumber("Chosen Reef Rot", p.getRotation().getDegrees());
                 }));
   }
 
@@ -277,25 +212,16 @@ private static Entry<Integer, Command> coolDynamicPathScoringCommand(
       PathConstraints constraints,
       Command prepareToScoreCommand,
       Command scoreCommand) {
-    // FIXME initialize a fresh instance of the prepareToScoreCommand each time!!
-    // OTHERWISE CODE WILL CRASH
-    Entry<Integer, Command> entry =
-        Map.entry(
-            target.getIndex(),
-            new SequentialCommandGroup(
-                AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
-                prepareToScoreCommand,
-                new SelectCommand<>(
-                    Map.ofEntries(
-                        Map.entry(0, AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0)),
-                        Map.entry(1, AutoBuilder.pathfindToPoseFlipped(target.getLowerScoringPose(), constraints, 0.0))),
-                        () -> {
-                            return DriverControls.Constants.prepareScoreSelctedIndex < 4 ? 1 : 0;
-                          }
-                ),
-                scoreCommand));
-
-    return entry;
+    return Map.entry(
+        target.getIndex(),
+        new SequentialCommandGroup(
+            AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
+            prepareToScoreCommand,
+            new ConditionalCommand(
+                AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
+                AutoBuilder.pathfindToPoseFlipped(target.getLowerScoringPose(), constraints, 0.0),
+                () -> DriverControls.Constants.prepareScoreSelctedIndex < 4),
+            scoreCommand));
   }
 
   private static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm) {
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
index 4b15e937..6a071ddb 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -12,9 +12,9 @@ public enum TargetPose {
   // PATHPLANNER WILL DECIDE WHETHER THE DESTINATION POSE IS
   // ON THE RED OR BLUE SIDE OF THE FIELD
 
-  ORIGIN(0, "O", "Origin", new Pose2d(0.0, 0.0, new Rotation2d(0)), 0),
-  FEEDER_R(1, "FR", "Feeder Right", new Pose2d(1.05, 1, new Rotation2d(50)), 0),
-  FEEDER_L(2, "FL", "Feeder Left", new Pose2d(1.05, 7, Rotation2d.fromDegrees(-50)), 0),
+  ORIGIN(0, "O", "Origin", new Pose2d(0.0, 0.0, new Rotation2d(0))),
+  FEEDER_R(1, "FR", "Feeder Right", new Pose2d(1.05, 1, new Rotation2d(50))),
+  FEEDER_L(2, "FL", "Feeder Left", new Pose2d(1.05, 7, Rotation2d.fromDegrees(-50))),
   REEF_A(3, "A", "Reef A", 0.0, true),
   REEF_B(4, "B", "Reef B", 0.0, false),
   REEF_C(5, "C", "Reef C", 60.0, true),
@@ -27,7 +27,7 @@ public enum TargetPose {
   REEF_J(12, "J", "Reef J", 240.0, false),
   REEF_K(13, "K", "Reef K", 300.0, true),
   REEF_L(14, "L", "Reef L", 300.0, false),
-  PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)), 0);
+  PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)));
 
   // REMEMBER: actual distance between reef poles is 13 inches,
   // currently using 9 inches because robot currently overshoots target pose during traversal
@@ -39,6 +39,15 @@ public enum TargetPose {
   // the original position was x: 3.16, y: 3.82
   private final Pose2d blueReefBPos = new Pose2d(new Translation2d(3.175, 3.82), new Rotation2d());
 
+  public static TargetPose getReefTargetWithAngle(double angleDegrees) {
+    int reefPosIdx = (int) (angleDegrees / 30) + 7;
+    if (reefPosIdx > 14) reefPosIdx -= 12;
+    for (TargetPose x : values()) {
+      if (x.getIndex() == reefPosIdx) return x;
+    }
+    throw new IllegalArgumentException(reefPosIdx + " does not translate to valid reef position.");
+  }
+
   /*
    *  Home positions (blue)
    * A:
@@ -144,7 +153,8 @@ public Pose2d getEndPose(boolean isRed) {
   public Pose2d getLowerScoringPose() {
     return this.lowerLevelPose;
   }
-  private TargetPose(int idx, String sName, String lName, Pose2d targetPose, int reefPosition) {
+
+  private TargetPose(int idx, String sName, String lName, Pose2d targetPose) {
     this.index = idx;
     this.shortName = sName;
     this.longName = lName;
@@ -152,16 +162,9 @@ private TargetPose(int idx, String sName, String lName, Pose2d targetPose, int r
 
     // TODO handle error case of accessing prep and end poses on non-reef position
 
-    // If this pose is not on the reef, don't set extra poses
-    if (reefPosition == 0) {
-      this.prepPose = null;
-      this.endPose = null;
-    } else {
-      double distance = (reefPosition < 0) ? -0.7 : 0.7;
-      this.prepPose = targetPose.transformBy(new Transform2d(0, distance, new Rotation2d()));
-      this.lowerLevelPose = this.pose.transformBy(new Transform2d(0.04445, 0, new Rotation2d()));
-      this.endPose = targetPose.transformBy(new Transform2d(-0.7, 0, new Rotation2d()));
-    }
+    this.lowerLevelPose = null;
+    this.prepPose = null;
+    this.endPose = null;
   }
 
   private TargetPose(
diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index e0c047f9..98b1753c 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -82,7 +82,7 @@ private double getDistanceToBestTarget() {
       return PhotonUtils.calculateDistanceToTargetMeters(
               robotToCamera.getZ(),
               fieldLayout.getTagPose(bestTarget.getFiducialId()).get().getZ(),
-              -robotToCamera.getRotation().getY(),
+              robotToCamera.getRotation().getY(),
               Units.degreesToRadians(bestTarget.getPitch()))
           + Constants.visionDistanceOffsetInMeters;
     }

From 544339ad0071d79747caad34c3b54f0754d5e2ba Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Thu, 13 Mar 2025 20:51:54 -0400
Subject: [PATCH 70/90] Changed Auto Source Path and lowered intake value in
 auto

---
 src/main/deploy/pathplanner/paths/F to Source X.path      | 8 ++++----
 src/main/deploy/pathplanner/paths/G to Source Y.path      | 8 ++++----
 src/main/deploy/pathplanner/paths/I to Source Y.path      | 8 ++++----
 src/main/deploy/pathplanner/paths/Source X to C.path      | 8 ++++----
 src/main/deploy/pathplanner/paths/Source Y to L.path      | 8 ++++----
 .../robot/config/game/reefscape2025/RobotConfigComp.java  | 2 +-
 6 files changed, 21 insertions(+), 21 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/F to Source X.path b/src/main/deploy/pathplanner/paths/F to Source X.path
index eb34c20c..7f68d0e6 100644
--- a/src/main/deploy/pathplanner/paths/F to Source X.path	
+++ b/src/main/deploy/pathplanner/paths/F to Source X.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.1766477272727274,
-        "y": 0.7393607954545461
+        "x": 1.196590909090909,
+        "y": 0.8590198863636362
       },
       "prevControl": {
-        "x": 2.3931818181818194,
-        "y": 1.0285369318181825
+        "x": 2.413125000000001,
+        "y": 1.1481960227272716
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/G to Source Y.path b/src/main/deploy/pathplanner/paths/G to Source Y.path
index 77b5293d..8d0aaa8a 100644
--- a/src/main/deploy/pathplanner/paths/G to Source Y.path	
+++ b/src/main/deploy/pathplanner/paths/G to Source Y.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.0370454545454546,
-        "y": 7.171036931818182
+        "x": 1.1267897727272727,
+        "y": 7.051377840909091
       },
       "prevControl": {
-        "x": 3.898892045454545,
-        "y": 6.642542613636365
+        "x": 3.9886363636363633,
+        "y": 6.522883522727274
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/I to Source Y.path b/src/main/deploy/pathplanner/paths/I to Source Y.path
index a7115930..ffdeb49f 100644
--- a/src/main/deploy/pathplanner/paths/I to Source Y.path	
+++ b/src/main/deploy/pathplanner/paths/I to Source Y.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.0370454545454546,
-        "y": 7.171036931818182
+        "x": 1.1267897727272727,
+        "y": 7.051377840909091
       },
       "prevControl": {
-        "x": 0.7422737733455748,
-        "y": 7.259538341361865
+        "x": 0.8320180915273928,
+        "y": 7.139879250452775
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Source X to C.path b/src/main/deploy/pathplanner/paths/Source X to C.path
index b2b156eb..01683d16 100644
--- a/src/main/deploy/pathplanner/paths/Source X to C.path	
+++ b/src/main/deploy/pathplanner/paths/Source X to C.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.1766477272727274,
-        "y": 0.7393607954545461
+        "x": 1.196590909090909,
+        "y": 0.8590198863636362
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.389856826038373,
-        "y": 0.869905348016272
+        "x": 1.4098000078565547,
+        "y": 0.989564438925362
       },
       "isLocked": false,
       "linkedName": "Source X"
diff --git a/src/main/deploy/pathplanner/paths/Source Y to L.path b/src/main/deploy/pathplanner/paths/Source Y to L.path
index e2ff3f5d..614dcc35 100644
--- a/src/main/deploy/pathplanner/paths/Source Y to L.path	
+++ b/src/main/deploy/pathplanner/paths/Source Y to L.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.0370454545454546,
-        "y": 7.171036931818182
+        "x": 1.1267897727272727,
+        "y": 7.051377840909091
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.180957379972435,
-        "y": 6.300892400897832
+        "x": 2.2707016981542534,
+        "y": 6.181233309988742
       },
       "isLocked": false,
       "linkedName": "Source Y"
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 06bc20f4..ebaa94fd 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -248,7 +248,7 @@ public RobotConfigComp() {
     NamedCommands.registerCommand(
         "Move Elevator to 0.8 meter", new ElevatorToPosition(elevator, () -> 0.8));
     NamedCommands.registerCommand(
-        "Move Elevator to 0.4 meter", new ElevatorToPosition(elevator, () -> 0.4));
+        "Move Elevator to 0.4 meter", new ElevatorToPosition(elevator, () -> 0.354));
     NamedCommands.registerCommand(
         "Move Arm for Intake", new ArmToPosition(coralArm, () -> -90).withTimeout(0));
     autoChooser = AutoBuilder.buildAutoChooser("Sit Still");

From c12a77735914dcfd2a9ae9e59923013259ebf24f Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Fri, 14 Mar 2025 08:58:21 -0400
Subject: [PATCH 71/90] intake controls changed based on driver input. Fixed
 coral arm jerking motion

---
 .../game/reefscape2025/RobotConfig.java       |  3 +-
 .../game/reefscape2025/RobotConfigComp.java   |  3 +-
 .../controls/arm/CoralArmControls.java        | 12 +++++
 .../combination/DriverAssistControls.java     | 10 ++--
 .../controls/combination/DriverControls.java  | 46 +++++++++----------
 5 files changed, 44 insertions(+), 30 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index 3bb0661f..1588d91e 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -214,6 +214,8 @@ public void configureBindings() {
     // Send vision-based odometry measurements to drive's odometry calculations
     vision.setVisionMeasurementConsumer(drive::addVisionMeasurement);
 
+    CoralArmControls.setupController(
+        coralArm, assistController); // move up to get reset encoder command
     DriveControls.setupController(drive, elevator, coralArm, mainController);
     DriveControls.setupAssistantController(drive, assistController);
     DriverAssistControls.setupController(elevator, coralArm, climberArm, assistController);
@@ -221,7 +223,6 @@ public void configureBindings() {
     PitControls.setupPitControls(elevator, coralArm, climberArm);
     CoralArmControls.setupController(coralArm, mainController);
     ElevatorControls.setupController(elevator, mainController);
-    CoralArmControls.setupController(coralArm, assistController);
     ElevatorControls.setupController(elevator, assistController);
     ClimberArmControls.setupController(climberArm, mainController);
 
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index ebaa94fd..aa97c087 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -2,7 +2,6 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
@@ -254,6 +253,6 @@ public RobotConfigComp() {
     autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
 
     // Start webcam
-    CameraServer.startAutomaticCapture();
+    // CameraServer.startAutomaticCapture();
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/arm/CoralArmControls.java b/src/main/java/frc/robot/subsystems/controls/arm/CoralArmControls.java
index ef4d18a5..508aa5f8 100644
--- a/src/main/java/frc/robot/subsystems/controls/arm/CoralArmControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/arm/CoralArmControls.java
@@ -17,6 +17,8 @@ public static class Constants {
     public static MotorAutoResetEncoderSettings autoZeroSettings =
         new MotorAutoResetEncoderSettings();
     public static Command autoCalibrateCommand;
+    public static Command autoCalibrateCommand2;
+    public static Command autoCalibrateCommand3;
   }
 
   // RIGHT POV = up arm
@@ -40,9 +42,19 @@ public static void setupController(Arm arm, CommandXboxController controller) {
     /* Add Auto Zero */
     Constants.autoCalibrateCommand =
         new MotorAutoResetEncoderCommand((Motor) arm, Constants.autoZeroSettings);
+    Constants.autoCalibrateCommand2 =
+        new MotorAutoResetEncoderCommand((Motor) arm, Constants.autoZeroSettings);
+    Constants.autoCalibrateCommand3 =
+        new MotorAutoResetEncoderCommand((Motor) arm, Constants.autoZeroSettings);
     SmartDashboard.putData(
         motorSubsystem.getName() + "/Commands/Auto Calibrate Coral Arm",
         Constants.autoCalibrateCommand);
+    SmartDashboard.putData(
+        motorSubsystem.getName() + "/Commands/Auto Calibrate Coral Arm Driver",
+        Constants.autoCalibrateCommand2);
+    SmartDashboard.putData(
+        motorSubsystem.getName() + "/Commands/Auto Calibrate Coral Arm Assist",
+        Constants.autoCalibrateCommand3);
 
     SmartDashboard.putData(
         motorSubsystem.getName() + "/Commands/Arm To -90", new ArmToPosition(arm, () -> -90));
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
index 738a8e3f..a26c3d41 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
@@ -49,13 +49,17 @@ public static void setupController(
             new ElevatorToPosition(elevator, () -> 1.3)));
 
     Command prepareIntakeCoralCommand =
-        new SequentialCommandGroup(
-            new ArmToPosition(coralArm, () -> -90).withTimeout(0),
-            new ElevatorToPosition(elevator, () -> 0.8));
+        new SequentialCommandGroup(new ElevatorToPosition(elevator, () -> 0.8));
+
+    SubsystemBase armSubsystem = (SubsystemBase) coralArm;
     controller
         .b()
         .onTrue(
             prepareIntakeCoralCommand
+                .andThen(
+                    (Command)
+                        SmartDashboard.getData(
+                            armSubsystem.getName() + "/Commands/Auto Calibrate Coral Arm Assist"))
                 .andThen(
                     new InstantCommand(
                         () -> {
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 9c8f9ebf..64c701cb 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -14,11 +14,8 @@
 import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.elevator.ElevatorCommand;
 import frc.robot.commands.common.elevator.ElevatorToPosition;
-import frc.robot.commands.common.motor.MotorAutoResetEncoderCommand;
-import frc.robot.subsystems.controls.arm.CoralArmControls;
 import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Elevator;
-import frc.robot.subsystems.interfaces.Motor;
 import frc.robot.subsystems.interfaces.SimpleMotor;
 import java.util.Map;
 
@@ -41,16 +38,16 @@ public static void setupController(
             });
 
     Command prepareIntakeCoralCommand =
-        new SequentialCommandGroup(
-            new ArmToPosition(coralArm, () -> -90).withTimeout(0),
-            new ElevatorToPosition(elevator, () -> 0.8));
+        new SequentialCommandGroup(new ElevatorToPosition(elevator, () -> 0.8));
+    SubsystemBase armSubsystem = (SubsystemBase) coralArm;
     controller
         .b()
         .onTrue(
             prepareIntakeCoralCommand
                 .andThen(
-                    new MotorAutoResetEncoderCommand(
-                        (Motor) coralArm, CoralArmControls.Constants.autoZeroSettings))
+                    (Command)
+                        SmartDashboard.getData(
+                            armSubsystem.getName() + "/Commands/Auto Calibrate Coral Arm Driver"))
                 .andThen(
                     new InstantCommand(
                         () -> {
@@ -60,7 +57,7 @@ public static void setupController(
 
     Command intakeCoralCommand = new ElevatorToPosition(elevator, () -> 0.345);
     Trigger scoreMode = new Trigger(() -> Constants.prepareScoreSelctedIndex >= 2);
-    controller.leftTrigger().and(scoreMode.negate()).and(ableToIntake).onTrue(intakeCoralCommand);
+    // controller.leftTrigger().and(scoreMode.negate()).and(ableToIntake).onTrue(intakeCoralCommand);
 
     Command score = new ArmToPosition(coralArm, () -> -10);
     controller.rightBumper().onTrue(score);
@@ -117,21 +114,22 @@ public static void setupController(
         "Driver " + "/Commands/Prepare To Score Command",
         getPrepareToScoreCommand(elevator, coralArm));
 
-    // climber
-    SubsystemBase climberSubsystem = (SubsystemBase) climber;
-    // prepare to climb
-    controller
-        .leftBumper()
-        .onTrue(
-            new InstantCommand(
-                () -> climber.setTargetPosition(climber.getSettings().maxPositionInRads),
-                climberSubsystem));
-
-    // climb
-    controller
-        .rightBumper()
-        .onTrue(new InstantCommand(() -> climber.setTargetPosition(11.6), climberSubsystem));
-
+    // // climber
+    // SubsystemBase climberSubsystem = (SubsystemBase) climber;
+    // // prepare to climb
+    // controller
+    //     .leftBumper()
+    //     .onTrue(
+    //         new InstantCommand(
+    //             () -> climber.setTargetPosition(climber.getSettings().maxPositionInRads),
+    //             climberSubsystem));
+
+    // // climb
+    // controller
+    //     .rightBumper()
+    //     .onTrue(new InstantCommand(() -> climber.setTargetPosition(11.6), climberSubsystem));
+
+    controller.leftBumper().and(ableToIntake).and(scoreMode.negate()).onTrue(intakeCoralCommand);
     // multi controll not workking in each subsystem inde
     controller.povUp().whileTrue(new ElevatorCommand(elevator, () -> 0.2));
 

From f45a829c5f6177c9839614ace4c83130b3722046 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Fri, 14 Mar 2025 11:43:39 -0400
Subject: [PATCH 72/90] Changed Auto to be more accurate

---
 .../deploy/pathplanner/autos/Score D L4.auto  | 82 +++++++++++++++++++
 .../autos/Score F L4 + Y + C L4 .auto         | 10 ++-
 .../autos/Score J L4 + Y + L L4 .auto         |  2 +-
 src/main/deploy/pathplanner/paths/Back D.path | 54 ++++++++++++
 .../pathplanner/paths/F to Source X.path      |  8 +-
 .../{Source X to C.path => Get to C.path}     | 16 ++--
 .../deploy/pathplanner/paths/Get to D.path    | 54 ++++++++++++
 .../pathplanner/paths/J to Source Y.path      | 54 ++++++++++++
 .../pathplanner/paths/Source X to Near C.path | 54 ++++++++++++
 .../pathplanner/paths/Start to Near D.path    | 54 ++++++++++++
 .../game/reefscape2025/RobotConfigComp.java   | 24 +++---
 11 files changed, 385 insertions(+), 27 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/Score D L4.auto
 create mode 100644 src/main/deploy/pathplanner/paths/Back D.path
 rename src/main/deploy/pathplanner/paths/{Source X to C.path => Get to C.path} (78%)
 create mode 100644 src/main/deploy/pathplanner/paths/Get to D.path
 create mode 100644 src/main/deploy/pathplanner/paths/J to Source Y.path
 create mode 100644 src/main/deploy/pathplanner/paths/Source X to Near C.path
 create mode 100644 src/main/deploy/pathplanner/paths/Start to Near D.path

diff --git a/src/main/deploy/pathplanner/autos/Score D L4.auto b/src/main/deploy/pathplanner/autos/Score D L4.auto
new file mode 100644
index 00000000..8524c5ef
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score D L4.auto	
@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near D"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to D"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back D"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto
index 70b063a9..18455b96 100644
--- a/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto	
@@ -101,7 +101,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 3.0
+            "waitTime": 2.0
           }
         },
         {
@@ -117,7 +117,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Source X to C"
+                  "pathName": "Source X to Near C"
                 }
               },
               {
@@ -161,6 +161,12 @@
             "name": "Move Arm 0 degrees"
           }
         },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to C"
+          }
+        },
         {
           "type": "path",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto
index 04e7269b..f1e73773 100644
--- a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
@@ -80,7 +80,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "I to Source Y"
+                  "pathName": "J to Source Y"
                 }
               },
               {
diff --git a/src/main/deploy/pathplanner/paths/Back D.path b/src/main/deploy/pathplanner/paths/Back D.path
new file mode 100644
index 00000000..621c8ad6
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Back D.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.9966025641025635,
+        "y": 2.8794230769230773
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8725664285837027,
+        "y": 2.6179661616351546
+      },
+      "isLocked": false,
+      "linkedName": "D Reef"
+    },
+    {
+      "anchor": {
+        "x": 3.821153846153846,
+        "y": 2.5904487179487172
+      },
+      "prevControl": {
+        "x": 3.9072673463703698,
+        "y": 2.8251495170109675
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/F to Source X.path b/src/main/deploy/pathplanner/paths/F to Source X.path
index 7f68d0e6..d04803e9 100644
--- a/src/main/deploy/pathplanner/paths/F to Source X.path	
+++ b/src/main/deploy/pathplanner/paths/F to Source X.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.196590909090909,
-        "y": 0.8590198863636362
+        "x": 1.3132692307692304,
+        "y": 0.9701282051282044
       },
       "prevControl": {
-        "x": 2.413125000000001,
-        "y": 1.1481960227272716
+        "x": 2.529803321678322,
+        "y": 1.25930434149184
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Source X to C.path b/src/main/deploy/pathplanner/paths/Get to C.path
similarity index 78%
rename from src/main/deploy/pathplanner/paths/Source X to C.path
rename to src/main/deploy/pathplanner/paths/Get to C.path
index 01683d16..00e35ded 100644
--- a/src/main/deploy/pathplanner/paths/Source X to C.path	
+++ b/src/main/deploy/pathplanner/paths/Get to C.path	
@@ -3,16 +3,16 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.196590909090909,
-        "y": 0.8590198863636362
+        "x": 3.5631410256410248,
+        "y": 2.7658974358974366
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.4098000078565547,
-        "y": 0.989564438925362
+        "x": 3.8289600484680144,
+        "y": 3.2496381820489777
       },
       "isLocked": false,
-      "linkedName": "Source X"
+      "linkedName": "Near C"
     },
     {
       "anchor": {
@@ -20,8 +20,8 @@
         "y": 3.0627414772727275
       },
       "prevControl": {
-        "x": 3.389460921642796,
-        "y": 2.7811564813741643
+        "x": 3.6085933269601185,
+        "y": 2.8424690993271704
       },
       "nextControl": null,
       "isLocked": false,
@@ -48,7 +48,7 @@
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 53.84
+    "rotation": 57.27
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to D.path b/src/main/deploy/pathplanner/paths/Get to D.path
new file mode 100644
index 00000000..cb7d7398
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Get to D.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.490897435897436,
+        "y": 2.9826282051282056
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8830769230769224,
+        "y": 2.88974358974359
+      },
+      "isLocked": false,
+      "linkedName": "Near D"
+    },
+    {
+      "anchor": {
+        "x": 3.9966025641025635,
+        "y": 2.8794230769230773
+      },
+      "prevControl": {
+        "x": 3.604423076923076,
+        "y": 2.8691025641025636
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "D Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 25.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/J to Source Y.path b/src/main/deploy/pathplanner/paths/J to Source Y.path
new file mode 100644
index 00000000..6ea44c2e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/J to Source Y.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.045624999999999,
+        "y": 5.376150568181817
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.31201704544956,
+        "y": 6.542826704540467
+      },
+      "isLocked": false,
+      "linkedName": "Near J"
+    },
+    {
+      "anchor": {
+        "x": 1.1267897727272727,
+        "y": 7.051377840909091
+      },
+      "prevControl": {
+        "x": 0.8320180915273928,
+        "y": 7.139879250452775
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Source Y"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.841814560191686
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Source X to Near C.path b/src/main/deploy/pathplanner/paths/Source X to Near C.path
new file mode 100644
index 00000000..777e8a2a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Source X to Near C.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3132692307692304,
+        "y": 0.9701282051282044
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.526478329534876,
+        "y": 1.1006727576899302
+      },
+      "isLocked": false,
+      "linkedName": "Source X"
+    },
+    {
+      "anchor": {
+        "x": 3.5631410256410248,
+        "y": 2.7658974358974366
+      },
+      "prevControl": {
+        "x": 3.2232269472838215,
+        "y": 2.4843124399988734
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near C"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 53.84
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to Near D.path b/src/main/deploy/pathplanner/paths/Start to Near D.path
new file mode 100644
index 00000000..050447cb
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Start to Near D.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.857613636363637,
+        "y": 1.9259801136363626
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.22474358974359,
+        "y": 1.5067948717948723
+      },
+      "isLocked": false,
+      "linkedName": "Left Start"
+    },
+    {
+      "anchor": {
+        "x": 3.490897435897436,
+        "y": 2.9826282051282056
+      },
+      "prevControl": {
+        "x": 2.789102564102564,
+        "y": 1.3932692307692307
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near D"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 25.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index aa97c087..b19287cc 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -50,18 +50,18 @@ public RobotConfigComp() {
     drive = new DriveSwerveYAGSL("yagsl/comp");
 
     // Cameras
-    // vision.addCamera(
-    //     new Camera(
-    //         "rear_cam",
-    //         new Transform3d(
-    //             new Translation3d(
-    //                 Units.inchesToMeters(0.295),
-    //                 Units.inchesToMeters(-11.443),
-    //                 Units.inchesToMeters(39.663)),
-    //             new Rotation3d(
-    //                 Units.degreesToRadians(12),
-    //                 Units.degreesToRadians(-33),
-    //                 Units.degreesToRadians(170)))));
+    vision.addCamera(
+        new Camera(
+            "rear_cam",
+            new Transform3d(
+                new Translation3d(
+                    Units.inchesToMeters(0.295),
+                    Units.inchesToMeters(-11.443),
+                    Units.inchesToMeters(39.663)),
+                new Rotation3d(
+                    Units.degreesToRadians(12),
+                    Units.degreesToRadians(-33),
+                    Units.degreesToRadians(170)))));
     // vision.addCamera(
     //     new Camera(
     //         "left_cam", // left

From 301f9d43a426a2d305c4e5fa836e24e30d88673d Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Fri, 14 Mar 2025 12:32:11 -0400
Subject: [PATCH 73/90] auto and new climber pos

---
 .../deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto    | 2 +-
 .../robot/config/game/reefscape2025/RobotConfigComp.java    | 6 ++++--
 .../robot/subsystems/controls/arm/ClimberArmControls.java   | 2 +-
 .../controls/combination/DriverAssistControls.java          | 2 +-
 4 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto
index f1e73773..e9c37780 100644
--- a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
@@ -143,7 +143,7 @@
                           {
                             "type": "named",
                             "data": {
-                              "name": "Move Elevator to 1.4 meter"
+                              "name": "Move Elevator to 1.553 meter"
                             }
                           }
                         ]
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index b19287cc..a34761ec 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -119,7 +119,7 @@ public RobotConfigComp() {
       ElevatorSettings elevatorSettings = new ElevatorSettings();
       elevatorSettings.minHeightInMeters = 0.09 + 0.02;
       elevatorSettings.maxHeightInMeters = 0.02 + 0.85 + 0.76;
-      elevatorSettings.startingHeightInMeters = 0.3; // The elevator height when piece is in intake
+      elevatorSettings.startingHeightInMeters = 0.345; // The elevator height when piece is in intake
       elevatorSettings.color = new Color8Bit(Color.kSilver);
       elevatorSettings.feedforward =
           new ElevatorFeedforward(0.010472, 0.17328, 0.16928, 0.010615); // SysID 2025-02-28
@@ -198,7 +198,7 @@ public RobotConfigComp() {
 
       SimpleMotorSettings simpleMotorSettings = new SimpleMotorSettings();
       simpleMotorSettings.minPositionInRads = 0;
-      simpleMotorSettings.maxPositionInRads = 36.1;
+      simpleMotorSettings.maxPositionInRads = 38.06;
       simpleMotorSettings.startingPositionInRads = simpleMotorSettings.minPositionInRads;
       simpleMotorSettings.color = new Color8Bit(Color.kRed);
       simpleMotorSettings.feedforward = new SimpleMotorFeedforward(0, 0, 0);
@@ -238,6 +238,8 @@ public RobotConfigComp() {
     // Auto(s)
     NamedCommands.registerCommand(
         "Move Elevator to 0.5 meter", new ElevatorToPosition(elevator, () -> 0.5));
+    NamedCommands.registerCommand(
+        "Move Elevator to 1.0 meter", new ElevatorToPosition(elevator, () -> 1.0));
     NamedCommands.registerCommand(
         "Move Elevator to 1.553 meter", new ElevatorToPosition(elevator, () -> 1.553));
     NamedCommands.registerCommand(
diff --git a/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java b/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
index 3fef0340..766686dc 100644
--- a/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
@@ -49,6 +49,6 @@ public static void setupController(SimpleMotor arm, CommandXboxController contro
     SmartDashboard.putData(
         armSubsystem.getName() + "/Commands/Climb",
         new InstantCommand(
-            () -> arm.setTargetPosition(arm.getSettings().minPositionInRads), armSubsystem));
+            () -> arm.setTargetPosition(18.52), armSubsystem));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
index a26c3d41..1fda5a28 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
@@ -116,6 +116,6 @@ public static void setupController(
     // climb
     controller
         .rightBumper()
-        .onTrue(new InstantCommand(() -> climber.setTargetPosition(11.6), climberSubsystem));
+        .onTrue(new InstantCommand(() -> climber.setTargetPosition(18.06), climberSubsystem));
   }
 }

From 3d8e200b3834270f6a17bbebc7181209b22b17e9 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Fri, 14 Mar 2025 13:21:20 -0400
Subject: [PATCH 74/90] update climber climb pos

---
 .../frc/robot/subsystems/controls/arm/ClimberArmControls.java   | 2 +-
 .../subsystems/controls/combination/DriverAssistControls.java   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java b/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
index 766686dc..c5085daf 100644
--- a/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
@@ -49,6 +49,6 @@ public static void setupController(SimpleMotor arm, CommandXboxController contro
     SmartDashboard.putData(
         armSubsystem.getName() + "/Commands/Climb",
         new InstantCommand(
-            () -> arm.setTargetPosition(18.52), armSubsystem));
+            () -> arm.setTargetPosition(18.06 - 0.0873), armSubsystem));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
index 1fda5a28..a0ed60bf 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
@@ -116,6 +116,6 @@ public static void setupController(
     // climb
     controller
         .rightBumper()
-        .onTrue(new InstantCommand(() -> climber.setTargetPosition(18.06), climberSubsystem));
+        .onTrue(new InstantCommand(() -> climber.setTargetPosition(18.06  - 0.0873), climberSubsystem));
   }
 }

From d22fa542f4989331a8cc74827461ca07f394da33 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Fri, 14 Mar 2025 14:21:46 -0400
Subject: [PATCH 75/90] Added midpoint on 2 piece auto

---
 .../autos/Score I L4 + Y + L L4 .auto         |  8 ++-
 .../autos/Score J L4 + Y + L L4 .auto         |  8 ++-
 src/main/deploy/pathplanner/paths/Back L.path | 12 ++---
 .../deploy/pathplanner/paths/Get to L.path    | 54 +++++++++++++++++++
 ...ce Y to L.path => Source Y to Near L.path} | 10 ++--
 5 files changed, 79 insertions(+), 13 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/Get to L.path
 rename src/main/deploy/pathplanner/paths/{Source Y to L.path => Source Y to Near L.path} (86%)

diff --git a/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto
index fb80f729..25c8c39c 100644
--- a/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
@@ -117,7 +117,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Source Y to L"
+                  "pathName": "Source Y to Near L"
                 }
               },
               {
@@ -161,6 +161,12 @@
             "name": "Move Arm 0 degrees"
           }
         },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to L"
+          }
+        },
         {
           "type": "path",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto
index e9c37780..01e49f0a 100644
--- a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
@@ -117,7 +117,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Source Y to L"
+                  "pathName": "Source Y to Near L"
                 }
               },
               {
@@ -161,6 +161,12 @@
             "name": "Move Arm 0 degrees"
           }
         },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to L"
+          }
+        },
         {
           "type": "path",
           "data": {
diff --git a/src/main/deploy/pathplanner/paths/Back L.path b/src/main/deploy/pathplanner/paths/Back L.path
index 77214bf4..4fc1873b 100644
--- a/src/main/deploy/pathplanner/paths/Back L.path	
+++ b/src/main/deploy/pathplanner/paths/Back L.path	
@@ -16,16 +16,16 @@
     },
     {
       "anchor": {
-        "x": 3.387692307692307,
-        "y": 5.480192307692307
+        "x": 3.5837820512820513,
+        "y": 5.284102564102564
       },
       "prevControl": {
-        "x": 3.5822727940278267,
-        "y": 5.2659375102373165
+        "x": 3.778362537617571,
+        "y": 5.069847766647573
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Near L"
     }
   ],
   "rotationTargets": [],
@@ -42,7 +42,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -57.27
+    "rotation": -57.265
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/paths/Get to L.path b/src/main/deploy/pathplanner/paths/Get to L.path
new file mode 100644
index 00000000..4eacee2e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Get to L.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5837820512820513,
+        "y": 5.284102564102564
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7396610397183667,
+        "y": 5.023019920057829
+      },
+      "isLocked": false,
+      "linkedName": "Near L"
+    },
+    {
+      "anchor": {
+        "x": 3.7194034090909094,
+        "y": 4.9972301136363635
+      },
+      "prevControl": {
+        "x": 3.565097644575375,
+        "y": 5.231798745830039
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "L Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -57.265
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -57.265
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Source Y to L.path b/src/main/deploy/pathplanner/paths/Source Y to Near L.path
similarity index 86%
rename from src/main/deploy/pathplanner/paths/Source Y to L.path
rename to src/main/deploy/pathplanner/paths/Source Y to Near L.path
index 614dcc35..d284fbd0 100644
--- a/src/main/deploy/pathplanner/paths/Source Y to L.path	
+++ b/src/main/deploy/pathplanner/paths/Source Y to Near L.path	
@@ -16,16 +16,16 @@
     },
     {
       "anchor": {
-        "x": 3.7194034090909094,
-        "y": 4.9972301136363635
+        "x": 3.5837820512820513,
+        "y": 5.284102564102564
       },
       "prevControl": {
-        "x": 3.406708270564879,
-        "y": 5.2623527460892765
+        "x": 3.271086912756021,
+        "y": 5.549225196555477
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "L Reef"
+      "linkedName": "Near L"
     }
   ],
   "rotationTargets": [],

From f01f3261b3405322c34cd41a243bb9664d534d91 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Fri, 14 Mar 2025 15:21:44 -0400
Subject: [PATCH 76/90] Fixed 2nd auto

---
 .../pathplanner/autos/Score F L4 + Y + C L4 .auto    | 10 +++++-----
 .../pathplanner/autos/Score I L4 + Y + L L4 .auto    | 12 ++++++------
 .../pathplanner/autos/Score J L4 + Y + L L4 .auto    |  2 +-
 .../deploy/pathplanner/paths/Source X to Near C.path |  6 +++---
 .../deploy/pathplanner/paths/Source Y to Near L.path |  6 +++---
 5 files changed, 18 insertions(+), 18 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto
index 18455b96..130ea35d 100644
--- a/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto	
@@ -143,7 +143,7 @@
                           {
                             "type": "named",
                             "data": {
-                              "name": "Move Elevator to 1.4 meter"
+                              "name": "Move Elevator to 1.553 meter"
                             }
                           }
                         ]
@@ -156,15 +156,15 @@
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "Move Arm 0 degrees"
+            "pathName": "Get to C"
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Get to C"
+            "name": "Move Arm 0 degrees"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto
index 25c8c39c..1cb04cb8 100644
--- a/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
@@ -101,7 +101,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 3.0
+            "waitTime": 2.0
           }
         },
         {
@@ -143,7 +143,7 @@
                           {
                             "type": "named",
                             "data": {
-                              "name": "Move Elevator to 1.4 meter"
+                              "name": "Move Elevator to 1.553 meter"
                             }
                           }
                         ]
@@ -156,15 +156,15 @@
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "Move Arm 0 degrees"
+            "pathName": "Get to L"
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Get to L"
+            "name": "Move Arm 0 degrees"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto
index 01e49f0a..62d8abe8 100644
--- a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
@@ -101,7 +101,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 3.0
+            "waitTime": 2.0
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/paths/Source X to Near C.path b/src/main/deploy/pathplanner/paths/Source X to Near C.path
index 777e8a2a..31db1a25 100644
--- a/src/main/deploy/pathplanner/paths/Source X to Near C.path	
+++ b/src/main/deploy/pathplanner/paths/Source X to Near C.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 53.84
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Source Y to Near L.path b/src/main/deploy/pathplanner/paths/Source Y to Near L.path
index d284fbd0..10e2ae5b 100644
--- a/src/main/deploy/pathplanner/paths/Source Y to Near L.path	
+++ b/src/main/deploy/pathplanner/paths/Source Y to Near L.path	
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": -53.841814560191686
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file

From 0b1e174f47290e81519f24aa18029f39704edb2b Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Fri, 14 Mar 2025 15:28:44 -0400
Subject: [PATCH 77/90] Fixed J 2 piece auto

---
 .../deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto  | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto
index 62d8abe8..827354cb 100644
--- a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
@@ -156,15 +156,15 @@
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "Move Arm 0 degrees"
+            "pathName": "Get to L"
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Get to L"
+            "name": "Move Arm 0 degrees"
           }
         },
         {

From 01e504aeef5779675b4695307a901f4325650e73 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Sat, 15 Mar 2025 13:56:14 -0400
Subject: [PATCH 78/90] Updated the 2 piece J auto for URI playoffs

---
 src/main/deploy/elastic-layout.json           | 31 ++++++++++---------
 .../autos/Score F L4 + Y + C L4 .auto         |  2 +-
 .../autos/Score I L4 + Y + L L4 .auto         |  2 +-
 .../autos/Score J L4 + Y + L L4 .auto         |  2 +-
 src/main/deploy/pathplanner/paths/Back J.path | 14 ++++-----
 src/main/deploy/pathplanner/paths/Back L.path |  8 ++---
 .../deploy/pathplanner/paths/Get to J.path    |  8 ++---
 .../deploy/pathplanner/paths/Get to L.path    |  8 ++---
 .../pathplanner/paths/J to Source Y.path      |  8 ++---
 .../pathplanner/paths/Source Y to Near L.path |  8 ++---
 .../pathplanner/paths/Start to Near J.path    |  8 ++---
 .../game/reefscape2025/RobotConfigComp.java   |  8 +++--
 .../controls/arm/ClimberArmControls.java      |  3 +-
 .../combination/DriverAssistControls.java     |  3 +-
 14 files changed, 59 insertions(+), 54 deletions(-)

diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json
index b5ac3a91..7c118ccc 100644
--- a/src/main/deploy/elastic-layout.json
+++ b/src/main/deploy/elastic-layout.json
@@ -108,8 +108,7 @@
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
-              "period": 0.06,
-              "rotation_turns": 0
+              "period": 0.06
             }
           }
         ]
@@ -384,7 +383,6 @@
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1184_Output_MJPEG_Server",
               "period": 0.06,
-              "rotation_turns": 0,
               "fps": 10,
               "resolution": [
                 160.0,
@@ -416,7 +414,6 @@
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
               "period": 0.06,
-              "rotation_turns": 0,
               "resolution": [
                 160.0,
                 120.0
@@ -489,6 +486,18 @@
               "data_type": "double",
               "show_submit_button": false
             }
+          },
+          {
+            "title": "USB Camera 0",
+            "x": 896.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/USB Camera 0",
+              "period": 0.06
+            }
           }
         ]
       }
@@ -1023,8 +1032,7 @@
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server",
-              "period": 0.06,
-              "rotation_turns": 0
+              "period": 0.06
             }
           },
           {
@@ -1036,8 +1044,7 @@
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1184_Output_MJPEG_Server",
-              "period": 0.06,
-              "rotation_turns": 0
+              "period": 0.06
             }
           },
           {
@@ -1049,8 +1056,7 @@
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1188_Output_MJPEG_Server",
-              "period": 0.06,
-              "rotation_turns": 0
+              "period": 0.06
             }
           },
           {
@@ -1062,8 +1068,7 @@
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
-              "period": 0.06,
-              "rotation_turns": 0
+              "period": 0.06
             }
           }
         ]
@@ -2451,7 +2456,6 @@
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1184_Output_MJPEG_Server",
               "period": 0.06,
-              "rotation_turns": 0,
               "fps": 10,
               "resolution": [
                 160.0,
@@ -2483,7 +2487,6 @@
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
               "period": 0.06,
-              "rotation_turns": 0,
               "resolution": [
                 160.0,
                 120.0
diff --git a/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto
index 130ea35d..394eeb7d 100644
--- a/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto	
@@ -101,7 +101,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 2.0
+            "waitTime": 1.75
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto
index 1cb04cb8..8a4bedf6 100644
--- a/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
@@ -101,7 +101,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 2.0
+            "waitTime": 1.75
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto
index 827354cb..909a2d8f 100644
--- a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
+++ b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
@@ -101,7 +101,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 2.0
+            "waitTime": 1.75
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/paths/Back J.path b/src/main/deploy/pathplanner/paths/Back J.path
index 07ea5fc9..7244bb25 100644
--- a/src/main/deploy/pathplanner/paths/Back J.path	
+++ b/src/main/deploy/pathplanner/paths/Back J.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 5.045624999999999,
-        "y": 5.376150568181817
+        "x": 5.090576923076923,
+        "y": 5.480192307692307
       },
       "prevControl": {
-        "x": 4.923232686600558,
-        "y": 5.147387493000958
+        "x": 4.968184609677482,
+        "y": 5.251429232511448
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
-    "maxAcceleration": 1.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": -119.99999999999999
   },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back L.path b/src/main/deploy/pathplanner/paths/Back L.path
index 4fc1873b..77506dfb 100644
--- a/src/main/deploy/pathplanner/paths/Back L.path	
+++ b/src/main/deploy/pathplanner/paths/Back L.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 3.5837820512820513,
-        "y": 5.284102564102564
+        "x": 3.387692307692307,
+        "y": 5.542115384615385
       },
       "prevControl": {
-        "x": 3.778362537617571,
-        "y": 5.069847766647573
+        "x": 3.5822727940278267,
+        "y": 5.327860587160394
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Get to J.path b/src/main/deploy/pathplanner/paths/Get to J.path
index 92d5a80b..cdc106fa 100644
--- a/src/main/deploy/pathplanner/paths/Get to J.path	
+++ b/src/main/deploy/pathplanner/paths/Get to J.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.045624999999999,
-        "y": 5.376150568181817
+        "x": 5.090576923076923,
+        "y": 5.480192307692307
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.895624999999999,
-        "y": 5.176150568181817
+        "x": 4.9405769230769225,
+        "y": 5.280192307692307
       },
       "isLocked": false,
       "linkedName": "Near J"
diff --git a/src/main/deploy/pathplanner/paths/Get to L.path b/src/main/deploy/pathplanner/paths/Get to L.path
index 4eacee2e..b7fb74db 100644
--- a/src/main/deploy/pathplanner/paths/Get to L.path	
+++ b/src/main/deploy/pathplanner/paths/Get to L.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.5837820512820513,
-        "y": 5.284102564102564
+        "x": 3.387692307692307,
+        "y": 5.542115384615385
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.7396610397183667,
-        "y": 5.023019920057829
+        "x": 3.5435712961286225,
+        "y": 5.28103274057065
       },
       "isLocked": false,
       "linkedName": "Near L"
diff --git a/src/main/deploy/pathplanner/paths/J to Source Y.path b/src/main/deploy/pathplanner/paths/J to Source Y.path
index 6ea44c2e..c3530530 100644
--- a/src/main/deploy/pathplanner/paths/J to Source Y.path	
+++ b/src/main/deploy/pathplanner/paths/J to Source Y.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.045624999999999,
-        "y": 5.376150568181817
+        "x": 5.090576923076923,
+        "y": 5.480192307692307
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.31201704544956,
-        "y": 6.542826704540467
+        "x": 6.356968968526483,
+        "y": 6.6468684440509564
       },
       "isLocked": false,
       "linkedName": "Near J"
diff --git a/src/main/deploy/pathplanner/paths/Source Y to Near L.path b/src/main/deploy/pathplanner/paths/Source Y to Near L.path
index 10e2ae5b..70f77b16 100644
--- a/src/main/deploy/pathplanner/paths/Source Y to Near L.path	
+++ b/src/main/deploy/pathplanner/paths/Source Y to Near L.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 3.5837820512820513,
-        "y": 5.284102564102564
+        "x": 3.387692307692307,
+        "y": 5.542115384615385
       },
       "prevControl": {
-        "x": 3.271086912756021,
-        "y": 5.549225196555477
+        "x": 3.0749971691662767,
+        "y": 5.807238017068298
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Start to Near J.path b/src/main/deploy/pathplanner/paths/Start to Near J.path
index 7de88cc3..3cb338dc 100644
--- a/src/main/deploy/pathplanner/paths/Start to Near J.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Near J.path	
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 5.045624999999999,
-        "y": 5.376150568181817
+        "x": 5.090576923076923,
+        "y": 5.480192307692307
       },
       "prevControl": {
-        "x": 5.414573863636363,
-        "y": 6.433139204545454
+        "x": 5.459525786713287,
+        "y": 6.537180944055944
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index a34761ec..c30bcafa 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -2,6 +2,7 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
@@ -119,7 +120,8 @@ public RobotConfigComp() {
       ElevatorSettings elevatorSettings = new ElevatorSettings();
       elevatorSettings.minHeightInMeters = 0.09 + 0.02;
       elevatorSettings.maxHeightInMeters = 0.02 + 0.85 + 0.76;
-      elevatorSettings.startingHeightInMeters = 0.345; // The elevator height when piece is in intake
+      elevatorSettings.startingHeightInMeters =
+          0.345; // The elevator height when piece is in intake
       elevatorSettings.color = new Color8Bit(Color.kSilver);
       elevatorSettings.feedforward =
           new ElevatorFeedforward(0.010472, 0.17328, 0.16928, 0.010615); // SysID 2025-02-28
@@ -245,7 +247,7 @@ public RobotConfigComp() {
     NamedCommands.registerCommand(
         "Move Arm 75 degrees", new ArmToPosition(coralArm, () -> 70).withTimeout(1.5));
     NamedCommands.registerCommand(
-        "Move Arm 0 degrees", new ArmToPosition(coralArm, () -> 0).withTimeout(1.5));
+        "Move Arm 0 degrees", new ArmToPosition(coralArm, () -> 0).withTimeout(1.0));
     NamedCommands.registerCommand(
         "Move Elevator to 0.8 meter", new ElevatorToPosition(elevator, () -> 0.8));
     NamedCommands.registerCommand(
@@ -255,6 +257,6 @@ public RobotConfigComp() {
     autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
 
     // Start webcam
-    // CameraServer.startAutomaticCapture();
+    CameraServer.startAutomaticCapture();
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java b/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
index c5085daf..dfe63bdd 100644
--- a/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
@@ -48,7 +48,6 @@ public static void setupController(SimpleMotor arm, CommandXboxController contro
             () -> arm.setTargetPosition(arm.getSettings().maxPositionInRads), armSubsystem));
     SmartDashboard.putData(
         armSubsystem.getName() + "/Commands/Climb",
-        new InstantCommand(
-            () -> arm.setTargetPosition(18.06 - 0.0873), armSubsystem));
+        new InstantCommand(() -> arm.setTargetPosition(18.06 - 0.0873), armSubsystem));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
index a0ed60bf..b7fa4cc1 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
@@ -116,6 +116,7 @@ public static void setupController(
     // climb
     controller
         .rightBumper()
-        .onTrue(new InstantCommand(() -> climber.setTargetPosition(18.06  - 0.0873), climberSubsystem));
+        .onTrue(
+            new InstantCommand(() -> climber.setTargetPosition(18.06 - 0.175), climberSubsystem));
   }
 }

From 91a6b38582a152f28ccbc5370f8f5afdf9dff463 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Wed, 19 Mar 2025 19:53:38 -0400
Subject: [PATCH 79/90] conditional midpt for l4

---
 .../robot/subsystems/controls/combination/DriverControls.java   | 2 +-
 .../java/frc/robot/subsystems/controls/drive/DriveControls.java | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 64c701cb..3b562e56 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -159,7 +159,7 @@ public static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm)
             Map.entry(
                 4,
                 new SequentialCommandGroup(
-                        new ElevatorToPosition(elevator, () -> 0.6),
+                        new ElevatorToPosition(elevator, () -> 0.6).unless(() -> elevator.getCurrentHeight() > 0.6),
                         new ParallelCommandGroup(
                             new ArmToPosition(coralArm, () -> 48).withTimeout(1.0),
                             new ElevatorToPosition(elevator, () -> 1.553)))
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index af27c302..c7512d7a 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -248,7 +248,7 @@ private static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm)
             Map.entry(
                 4,
                 new SequentialCommandGroup(
-                        new ElevatorToPosition(elevator, () -> 0.6),
+                        new ElevatorToPosition(elevator, () -> 0.6).unless(() -> elevator.getCurrentHeight() > 0.6),
                         new ParallelCommandGroup(
                             new ArmToPosition(coralArm, () -> 48).withTimeout(1.0),
                             new ElevatorToPosition(elevator, () -> 1.553)))

From 2f16e389ca85247622a5324ce6e68112652b4586 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Wed, 19 Mar 2025 20:03:41 -0400
Subject: [PATCH 80/90] drive accel

---
 src/main/java/frc/robot/io/interfaces/DriveIO.java            | 4 ++++
 .../subsystems/implementations/drive/DriveSwerveYAGSL.java    | 4 ++--
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/io/interfaces/DriveIO.java b/src/main/java/frc/robot/io/interfaces/DriveIO.java
index d3887636..9e68e216 100644
--- a/src/main/java/frc/robot/io/interfaces/DriveIO.java
+++ b/src/main/java/frc/robot/io/interfaces/DriveIO.java
@@ -2,6 +2,8 @@
 
 import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation3d;
+
 import org.littletonrobotics.junction.AutoLog;
 import swervelib.SwerveDrive;
 
@@ -16,6 +18,7 @@ public static class DriveIOInputs {
     public double flippedPoseX = 0.0;
     public double flippedPoseY = 0.0;
     public double flippedPoseRotInDegrees = 0.0;
+    public Translation3d currentAcceleration = new Translation3d();
   }
 
   /** Updates the set of loggable inputs. */
@@ -28,6 +31,7 @@ public void updateInputs(DriveIOInputs inputs, SwerveDrive swerveDrive) {
     inputs.flippedPoseX = inputs.flippedPose.getTranslation().getX();
     inputs.flippedPoseY = inputs.flippedPose.getTranslation().getY();
     inputs.flippedPoseRotInDegrees = inputs.flippedPose.getRotation().getDegrees();
+    inputs.currentAcceleration = swerveDrive.getAccel().get();
   }
   // Other methods for controlling the drive subsystem...
 }
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index d9790cfa..f57fe1b0 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -25,6 +25,7 @@
 import swervelib.SwerveDriveTest;
 import swervelib.parser.SwerveParser;
 import swervelib.telemetry.SwerveDriveTelemetry;
+import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 
 public class DriveSwerveYAGSL extends DriveBase {
   public static class Constants {
@@ -49,7 +50,7 @@ public DriveSwerveYAGSL(String configPath) {
     super("YAGSL");
     swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), configPath);
 
-    // SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
+    SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
     try {
       swerveDrive =
           new SwerveParser(swerveJsonDirectory)
@@ -309,7 +310,6 @@ public void lockPose() {
   public void periodic() {
     io.updateInputs(inputs, swerveDrive);
     Logger.processInputs("Drive", inputs);
-
     // Because the asynchronous odometry updates have been disabled,
     // we invoke updates manually
     swerveDrive.updateOdometry();

From 0c74125f91d9469c5ff9ce79be6bdfb07f69a144 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Thu, 20 Mar 2025 23:09:04 -0400
Subject: [PATCH 81/90] Added 2 Commands to SmartDashboard to test odometry in
 DriveControls. Ran spotlessApply

---
 .../commands/common/drive/DriveToPositionX.java      |  8 ++++++--
 src/main/java/frc/robot/io/interfaces/DriveIO.java   |  1 -
 .../controls/combination/DriverControls.java         |  3 ++-
 .../subsystems/controls/drive/DriveControls.java     | 12 +++++++++++-
 4 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
index 6b07e923..eccb2018 100644
--- a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
+++ b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
@@ -29,12 +29,16 @@ public void initialize() {
 
   @Override
   public void execute() {
-    drive.runVelocity(new ChassisSpeeds(-drive.getMaxLinearSpeed(), 0, 0));
+    drive.runVelocity(
+        new ChassisSpeeds(
+            Math.signum(distanceMeters.getAsDouble()) * drive.getMaxLinearSpeed(), 0, 0));
   }
 
   @Override
   public boolean isFinished() {
-    return targetPose.relativeTo(drive.getPose()).getX() > 0;
+    double currentX = drive.getPose().getX();
+    double targetX = targetPose.getX();
+    return Math.abs(currentX - targetX) >= Math.abs(distanceMeters.getAsDouble());
   }
 
   @Override
diff --git a/src/main/java/frc/robot/io/interfaces/DriveIO.java b/src/main/java/frc/robot/io/interfaces/DriveIO.java
index 9e68e216..705bda15 100644
--- a/src/main/java/frc/robot/io/interfaces/DriveIO.java
+++ b/src/main/java/frc/robot/io/interfaces/DriveIO.java
@@ -3,7 +3,6 @@
 import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation3d;
-
 import org.littletonrobotics.junction.AutoLog;
 import swervelib.SwerveDrive;
 
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 3b562e56..42e34742 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -159,7 +159,8 @@ public static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm)
             Map.entry(
                 4,
                 new SequentialCommandGroup(
-                        new ElevatorToPosition(elevator, () -> 0.6).unless(() -> elevator.getCurrentHeight() > 0.6),
+                        new ElevatorToPosition(elevator, () -> 0.6)
+                            .unless(() -> elevator.getCurrentHeight() > 0.6),
                         new ParallelCommandGroup(
                             new ArmToPosition(coralArm, () -> 48).withTimeout(1.0),
                             new ElevatorToPosition(elevator, () -> 1.553)))
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index c7512d7a..12530203 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -18,6 +18,8 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.drive.DriveCommand;
+import frc.robot.commands.common.drive.DriveToPositionX;
+import frc.robot.commands.common.drive.DriveToYaw;
 import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.subsystems.controls.combination.DriverControls;
 import frc.robot.subsystems.interfaces.Arm;
@@ -31,6 +33,7 @@
 public class DriveControls {
 
   protected static TargetPose chosenTarget = TargetPose.REEF_A;
+  static int value = 0;
 
   public static void setupController(
       Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
@@ -59,6 +62,12 @@ public static void setupController(
         new PathConstraints(
             drive.getMaxLinearSpeed(), 1.5, drive.getMaxAngularSpeed(), Math.PI / 4);
 
+    SmartDashboard.putData("Go Forward 1 Meter", new DriveToPositionX(drive, () -> 1.0));
+
+    SmartDashboard.putData(
+        "Go Forward 1 Meter and Turn 180",
+        new SequentialCommandGroup(
+            new DriveToPositionX(drive, () -> 1.0), new DriveToYaw(drive, () -> 180.0)));
     // Temporary UI to allow user to modify destination on-the-fly
     SendableChooser<TargetPose> chooser = new SendableChooser<>();
     // chooser.setDefaultOption("Origin", TargetPose.ORIGIN);
@@ -248,7 +257,8 @@ private static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm)
             Map.entry(
                 4,
                 new SequentialCommandGroup(
-                        new ElevatorToPosition(elevator, () -> 0.6).unless(() -> elevator.getCurrentHeight() > 0.6),
+                        new ElevatorToPosition(elevator, () -> 0.6)
+                            .unless(() -> elevator.getCurrentHeight() > 0.6),
                         new ParallelCommandGroup(
                             new ArmToPosition(coralArm, () -> 48).withTimeout(1.0),
                             new ElevatorToPosition(elevator, () -> 1.553)))

From dea518f8790cd56795828b5f2d9ecbceeb6a77bc Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Mon, 24 Mar 2025 18:25:30 -0400
Subject: [PATCH 82/90] updated rear cam placement

---
 .../robot/config/game/reefscape2025/RobotConfigComp.java    | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index c30bcafa..69a83236 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -56,13 +56,13 @@ public RobotConfigComp() {
             "rear_cam",
             new Transform3d(
                 new Translation3d(
-                    Units.inchesToMeters(0.295),
+                    Units.inchesToMeters(0),
                     Units.inchesToMeters(-11.443),
                     Units.inchesToMeters(39.663)),
                 new Rotation3d(
-                    Units.degreesToRadians(12),
+                    Units.degreesToRadians(0),
                     Units.degreesToRadians(-33),
-                    Units.degreesToRadians(170)))));
+                    Units.degreesToRadians(180)))));
     // vision.addCamera(
     //     new Camera(
     //         "left_cam", // left

From 06d2850305e2d357909167f04faaa694c3a71f6f Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Mon, 24 Mar 2025 18:32:16 -0400
Subject: [PATCH 83/90] fix sim

---
 src/main/java/frc/robot/io/interfaces/DriveIO.java | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/io/interfaces/DriveIO.java b/src/main/java/frc/robot/io/interfaces/DriveIO.java
index 705bda15..7a1ff3d3 100644
--- a/src/main/java/frc/robot/io/interfaces/DriveIO.java
+++ b/src/main/java/frc/robot/io/interfaces/DriveIO.java
@@ -3,6 +3,8 @@
 import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation3d;
+import frc.robot.Robot;
+
 import org.littletonrobotics.junction.AutoLog;
 import swervelib.SwerveDrive;
 
@@ -30,7 +32,9 @@ public void updateInputs(DriveIOInputs inputs, SwerveDrive swerveDrive) {
     inputs.flippedPoseX = inputs.flippedPose.getTranslation().getX();
     inputs.flippedPoseY = inputs.flippedPose.getTranslation().getY();
     inputs.flippedPoseRotInDegrees = inputs.flippedPose.getRotation().getDegrees();
-    inputs.currentAcceleration = swerveDrive.getAccel().get();
+    if(!Robot.isSimulation()) {
+      inputs.currentAcceleration = swerveDrive.getAccel().get();
+    }
   }
   // Other methods for controlling the drive subsystem...
 }

From a83110496b5b6e8eb77c22474fa92b0fe1d2cc6f Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Mon, 24 Mar 2025 19:33:58 -0400
Subject: [PATCH 84/90] Made go in a square path for drivetrain testing Also
 fixed rear cam x and y

---
 .../common/drive/DriveToPositionX.java        |  6 +++++-
 .../commands/common/drive/DriveToYaw.java     | 20 ++++++-------------
 .../game/reefscape2025/RobotConfigComp.java   |  4 ++--
 .../java/frc/robot/io/interfaces/DriveIO.java |  3 +--
 .../controls/drive/DriveControls.java         | 12 +++++++++++
 5 files changed, 26 insertions(+), 19 deletions(-)

diff --git a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
index eccb2018..5e5eca51 100644
--- a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
+++ b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
@@ -23,12 +23,14 @@ public DriveToPositionX(Drive drive, DoubleSupplier distanceMeters) {
 
   @Override
   public void initialize() {
+    System.out.println("  START: " + this.getClass().getSimpleName());
     double d = this.distanceMeters.getAsDouble();
     targetPose = drive.getPose().transformBy(new Transform2d(d, 0, new Rotation2d()));
   }
 
   @Override
   public void execute() {
+
     drive.runVelocity(
         new ChassisSpeeds(
             Math.signum(distanceMeters.getAsDouble()) * drive.getMaxLinearSpeed(), 0, 0));
@@ -38,7 +40,9 @@ public void execute() {
   public boolean isFinished() {
     double currentX = drive.getPose().getX();
     double targetX = targetPose.getX();
-    return Math.abs(currentX - targetX) >= Math.abs(distanceMeters.getAsDouble());
+    double distanceLeft = currentX - targetX;
+    System.out.println("Distance Away: " + distanceLeft);
+    return Math.abs(distanceLeft) >= Math.abs(distanceMeters.getAsDouble());
   }
 
   @Override
diff --git a/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java b/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
index 187aafcf..b79a4222 100644
--- a/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
+++ b/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
@@ -30,19 +30,11 @@ public DriveToYaw(Drive drive, DoubleSupplier yawDegrees) {
 
   @Override
   public void initialize() {
+    // System.out.println("  START: " + this.getClass().getSimpleName());
     targetYaw = this.yawDegrees.getAsDouble();
     turnPID.reset();
     turnPID.setSetpoint(targetYaw);
     timer.reset();
-    if (Constants.debugCommands) {
-      System.out.println(
-          "START: "
-              + this.getClass().getSimpleName()
-              + " yaw: "
-              + targetYaw
-              + " currentYaw: "
-              + drive.getAngle());
-    }
   }
 
   @Override
@@ -50,18 +42,18 @@ public void execute() {
     double rotate = turnPID.calculate(drive.getAngle());
     ChassisSpeeds speeds = new ChassisSpeeds(0, 0, rotate * drive.getMaxAngularSpeed());
     drive.runVelocity(speeds);
+    targetYaw = this.yawDegrees.getAsDouble();
+    double yawLeft = targetYaw - drive.getAngle();
+    System.out.println("Angle Away: " + yawLeft);
   }
 
   @Override
   public boolean isFinished() {
     if (turnPID.atSetpoint()) {
-      if (timer.get() >= DriveBase.Constants.pidSettlingTimeInSeconds) {
-        return true;
-      }
+      return true;
     } else {
-      timer.reset();
+      return false;
     }
-    return false;
   }
 
   @Override
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 69a83236..b537a001 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -47,7 +47,7 @@ public RobotConfigComp() {
     Drive.Constants.rotatePidKp = 0.025;
     Drive.Constants.rotatePidKi = 0.0;
     Drive.Constants.rotatePidKd = 0.0;
-    DriveBase.Constants.rotatePidErrorInDegrees = 1;
+    DriveBase.Constants.rotatePidErrorInDegrees = 2;
     drive = new DriveSwerveYAGSL("yagsl/comp");
 
     // Cameras
@@ -56,8 +56,8 @@ public RobotConfigComp() {
             "rear_cam",
             new Transform3d(
                 new Translation3d(
+                    Units.inchesToMeters(0.295),
                     Units.inchesToMeters(0),
-                    Units.inchesToMeters(-11.443),
                     Units.inchesToMeters(39.663)),
                 new Rotation3d(
                     Units.degreesToRadians(0),
diff --git a/src/main/java/frc/robot/io/interfaces/DriveIO.java b/src/main/java/frc/robot/io/interfaces/DriveIO.java
index 7a1ff3d3..8f671f58 100644
--- a/src/main/java/frc/robot/io/interfaces/DriveIO.java
+++ b/src/main/java/frc/robot/io/interfaces/DriveIO.java
@@ -4,7 +4,6 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import frc.robot.Robot;
-
 import org.littletonrobotics.junction.AutoLog;
 import swervelib.SwerveDrive;
 
@@ -32,7 +31,7 @@ public void updateInputs(DriveIOInputs inputs, SwerveDrive swerveDrive) {
     inputs.flippedPoseX = inputs.flippedPose.getTranslation().getX();
     inputs.flippedPoseY = inputs.flippedPose.getTranslation().getY();
     inputs.flippedPoseRotInDegrees = inputs.flippedPose.getRotation().getDegrees();
-    if(!Robot.isSimulation()) {
+    if (!Robot.isSimulation()) {
       inputs.currentAcceleration = swerveDrive.getAccel().get();
     }
   }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 12530203..919bb5ca 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -68,6 +68,18 @@ public static void setupController(
         "Go Forward 1 Meter and Turn 180",
         new SequentialCommandGroup(
             new DriveToPositionX(drive, () -> 1.0), new DriveToYaw(drive, () -> 180.0)));
+
+    SmartDashboard.putData(
+        "Go in a Square Path",
+        new SequentialCommandGroup(
+            new DriveToPositionX(drive, () -> 1.0),
+            new DriveToYaw(drive, () -> 90),
+            new DriveToPositionX(drive, () -> 1.0),
+            new DriveToYaw(drive, () -> 180),
+            new DriveToPositionX(drive, () -> 1.0),
+            new DriveToYaw(drive, () -> 270),
+            new DriveToPositionX(drive, () -> 1.0),
+            new DriveToYaw(drive, () -> 360)));
     // Temporary UI to allow user to modify destination on-the-fly
     SendableChooser<TargetPose> chooser = new SendableChooser<>();
     // chooser.setDefaultOption("Origin", TargetPose.ORIGIN);

From 330e00df86d7ccbea1f6108e03915efd2b9877d2 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Mon, 24 Mar 2025 19:37:11 -0400
Subject: [PATCH 85/90] change gear ratio

---
 .../frc/robot/config/game/reefscape2025/RobotConfigComp.java    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 69a83236..d2b7dfc9 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -106,7 +106,7 @@ public RobotConfigComp() {
     // Elevator
     {
       MotorIOBaseSettings motorSettings = new MotorIOBaseSettings();
-      motorSettings.motor.gearing = 9; /* 2x 3:1 gear boxes */
+      motorSettings.motor.gearing = 5; /* 2x 3:1 gear boxes */
       motorSettings.motor.inverted = true;
       // TODO: Get the DIO ports
       // motorSettings.forwardLimitChannel = 7;

From 601093bb1c253bce13284e9b1e49622786149554 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Mon, 24 Mar 2025 19:39:20 -0400
Subject: [PATCH 86/90] chnage comment and spotless

---
 .../frc/robot/config/game/reefscape2025/RobotConfigComp.java   | 2 +-
 src/main/java/frc/robot/io/interfaces/DriveIO.java             | 3 +--
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index d2b7dfc9..1647970a 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -106,7 +106,7 @@ public RobotConfigComp() {
     // Elevator
     {
       MotorIOBaseSettings motorSettings = new MotorIOBaseSettings();
-      motorSettings.motor.gearing = 5; /* 2x 3:1 gear boxes */
+      motorSettings.motor.gearing = 5; /* 1x 5:1 gear boxes */
       motorSettings.motor.inverted = true;
       // TODO: Get the DIO ports
       // motorSettings.forwardLimitChannel = 7;
diff --git a/src/main/java/frc/robot/io/interfaces/DriveIO.java b/src/main/java/frc/robot/io/interfaces/DriveIO.java
index 7a1ff3d3..8f671f58 100644
--- a/src/main/java/frc/robot/io/interfaces/DriveIO.java
+++ b/src/main/java/frc/robot/io/interfaces/DriveIO.java
@@ -4,7 +4,6 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import frc.robot.Robot;
-
 import org.littletonrobotics.junction.AutoLog;
 import swervelib.SwerveDrive;
 
@@ -32,7 +31,7 @@ public void updateInputs(DriveIOInputs inputs, SwerveDrive swerveDrive) {
     inputs.flippedPoseX = inputs.flippedPose.getTranslation().getX();
     inputs.flippedPoseY = inputs.flippedPose.getTranslation().getY();
     inputs.flippedPoseRotInDegrees = inputs.flippedPose.getRotation().getDegrees();
-    if(!Robot.isSimulation()) {
+    if (!Robot.isSimulation()) {
       inputs.currentAcceleration = swerveDrive.getAccel().get();
     }
   }

From 260595bbce32dc8bb72971f425118d58bf8aa09d Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Wed, 26 Mar 2025 20:37:36 -0400
Subject: [PATCH 87/90] lower speeds

---
 .../commands/common/drive/DriveToPositionX.java  | 12 +++++++-----
 .../robot/commands/common/drive/DriveToYaw.java  |  2 +-
 .../subsystems/controls/drive/DriveControls.java | 16 ++++++++++++----
 3 files changed, 20 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
index 5e5eca51..310b297f 100644
--- a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
+++ b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
@@ -33,16 +34,17 @@ public void execute() {
 
     drive.runVelocity(
         new ChassisSpeeds(
-            Math.signum(distanceMeters.getAsDouble()) * drive.getMaxLinearSpeed(), 0, 0));
+            Math.signum(distanceMeters.getAsDouble()) * (drive.getMaxLinearSpeed()), 0, 0));
   }
 
   @Override
   public boolean isFinished() {
-    double currentX = drive.getPose().getX();
-    double targetX = targetPose.getX();
-    double distanceLeft = currentX - targetX;
+    // double currentX = drive.getPose().getX();
+    
+    // double targetX = targetPose.getX();
+    double distanceLeft = new Translation2d().getDistance(drive.getPose().relativeTo(targetPose).getTranslation());
     System.out.println("Distance Away: " + distanceLeft);
-    return Math.abs(distanceLeft) >= Math.abs(distanceMeters.getAsDouble());
+    return Math.abs(distanceLeft) <= 0.1;
   }
 
   @Override
diff --git a/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java b/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
index b79a4222..4bbf0fc2 100644
--- a/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
+++ b/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
@@ -40,7 +40,7 @@ public void initialize() {
   @Override
   public void execute() {
     double rotate = turnPID.calculate(drive.getAngle());
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, rotate * drive.getMaxAngularSpeed());
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, rotate * (drive.getMaxAngularSpeed()/8));
     drive.runVelocity(speeds);
     targetYaw = this.yawDegrees.getAsDouble();
     double yawLeft = targetYaw - drive.getAngle();
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 919bb5ca..2138f547 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj2.command.SelectCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.drive.DriveCommand;
@@ -73,13 +74,20 @@ public static void setupController(
         "Go in a Square Path",
         new SequentialCommandGroup(
             new DriveToPositionX(drive, () -> 1.0),
-            new DriveToYaw(drive, () -> 90),
+            new WaitCommand(0.5),
+            new DriveToYaw(drive, () -> 90).withTimeout(2),
+            new WaitCommand(0.5),
             new DriveToPositionX(drive, () -> 1.0),
-            new DriveToYaw(drive, () -> 180),
+            new WaitCommand(0.5),
+            new DriveToYaw(drive, () -> 180).withTimeout(2),
+            new WaitCommand(0.5),
             new DriveToPositionX(drive, () -> 1.0),
-            new DriveToYaw(drive, () -> 270),
+            new WaitCommand(0.5),
+            new DriveToYaw(drive, () -> -90).withTimeout(2),
+            new WaitCommand(0.5),
             new DriveToPositionX(drive, () -> 1.0),
-            new DriveToYaw(drive, () -> 360)));
+            new WaitCommand(0.5),
+            new DriveToYaw(drive, () -> 0).withTimeout(2)));
     // Temporary UI to allow user to modify destination on-the-fly
     SendableChooser<TargetPose> chooser = new SendableChooser<>();
     // chooser.setDefaultOption("Origin", TargetPose.ORIGIN);

From c116fa84c1612d4a7c0a3b131efc4a0683bd69a1 Mon Sep 17 00:00:00 2001
From: joshuman <joshuamanoj2008@gmail.com>
Date: Wed, 2 Apr 2025 19:18:28 -0400
Subject: [PATCH 88/90] increased threshold

---
 .../frc/robot/commands/common/drive/DriveToPositionX.java  | 7 ++++---
 .../java/frc/robot/commands/common/drive/DriveToYaw.java   | 2 +-
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
index 310b297f..0d343ddd 100644
--- a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
+++ b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
@@ -40,11 +40,12 @@ public void execute() {
   @Override
   public boolean isFinished() {
     // double currentX = drive.getPose().getX();
-    
+
     // double targetX = targetPose.getX();
-    double distanceLeft = new Translation2d().getDistance(drive.getPose().relativeTo(targetPose).getTranslation());
+    double distanceLeft =
+        new Translation2d().getDistance(drive.getPose().relativeTo(targetPose).getTranslation());
     System.out.println("Distance Away: " + distanceLeft);
-    return Math.abs(distanceLeft) <= 0.1;
+    return Math.abs(distanceLeft) <= 0.2;
   }
 
   @Override
diff --git a/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java b/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
index 4bbf0fc2..2c07cf42 100644
--- a/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
+++ b/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
@@ -40,7 +40,7 @@ public void initialize() {
   @Override
   public void execute() {
     double rotate = turnPID.calculate(drive.getAngle());
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, rotate * (drive.getMaxAngularSpeed()/8));
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, rotate * (drive.getMaxAngularSpeed() / 8));
     drive.runVelocity(speeds);
     targetYaw = this.yawDegrees.getAsDouble();
     double yawLeft = targetYaw - drive.getAngle();

From d9d2f7f4d8010ab653fa8d5afde336582d013a34 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Mon, 6 Oct 2025 19:38:32 -0400
Subject: [PATCH 89/90] stub climber on comp

---
 .../game/reefscape2025/RobotConfigComp.java   | 58 ++++++------
 vendordeps/AdvantageKit.json                  |  6 +-
 ....2.4.json => PathplannerLib-2025.2.7.json} |  8 +-
 ....3.1.json => Phoenix6-frc2025-latest.json} | 92 ++++++++++++-------
 .../{REVLib-2025.0.2.json => REVLib.json}     | 12 +--
 vendordeps/ThriftyLib.json                    |  4 +-
 vendordeps/maple-sim.json                     |  4 +-
 vendordeps/photonlib.json                     | 12 +--
 8 files changed, 113 insertions(+), 83 deletions(-)
 rename vendordeps/{PathplannerLib-2025.2.4.json => PathplannerLib-2025.2.7.json} (87%)
 rename vendordeps/{Phoenix6-25.3.1.json => Phoenix6-frc2025-latest.json} (86%)
 rename vendordeps/{REVLib-2025.0.2.json => REVLib.json} (90%)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index ab1cac0c..cd8b6779 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -41,7 +41,7 @@ public class RobotConfigComp extends RobotConfig {
   private final AddressableLEDBuffer ledBuffer;
 
   public RobotConfigComp() {
-    super(false, false, false, false, false, true, false);
+    super(false, false, false, false, false, true, true);
 
     // Comp has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
@@ -189,38 +189,38 @@ public RobotConfigComp() {
     }
 
     // climber
-    {
-      MotorIOBaseSettings motorSettings = new MotorIOBaseSettings();
-      // 25:1 gear box ratio
-      motorSettings.motor.gearing = 25;
-      motorSettings.motor.inverted = false;
-      motorSettings.pid = new PIDController(1.0, 0, 0);
-      motorSettings.reverseLimitChannel = 1;
-      motorSettings.reverseLimitNegate = true;
+    // {
+    //   MotorIOBaseSettings motorSettings = new MotorIOBaseSettings();
+    //   // 25:1 gear box ratio
+    //   motorSettings.motor.gearing = 25;
+    //   motorSettings.motor.inverted = false;
+    //   motorSettings.pid = new PIDController(1.0, 0, 0);
+    //   motorSettings.reverseLimitChannel = 1;
+    //   motorSettings.reverseLimitNegate = true;
 
-      SimpleMotorSettings simpleMotorSettings = new SimpleMotorSettings();
-      simpleMotorSettings.minPositionInRads = 0;
-      simpleMotorSettings.maxPositionInRads = 38.06;
-      simpleMotorSettings.startingPositionInRads = simpleMotorSettings.minPositionInRads;
-      simpleMotorSettings.color = new Color8Bit(Color.kRed);
-      simpleMotorSettings.feedforward = new SimpleMotorFeedforward(0, 0, 0);
-      simpleMotorSettings.motor = DCMotor.getNEO(1);
+    //   SimpleMotorSettings simpleMotorSettings = new SimpleMotorSettings();
+    //   simpleMotorSettings.minPositionInRads = 0;
+    //   simpleMotorSettings.maxPositionInRads = 38.06;
+    //   simpleMotorSettings.startingPositionInRads = simpleMotorSettings.minPositionInRads;
+    //   simpleMotorSettings.color = new Color8Bit(Color.kRed);
+    //   simpleMotorSettings.feedforward = new SimpleMotorFeedforward(0, 0, 0);
+    //   simpleMotorSettings.motor = DCMotor.getNEO(1);
 
-      TalonFxSettings settings = new TalonFxSettings();
-      settings.canId = 50;
+    //   TalonFxSettings settings = new TalonFxSettings();
+    //   settings.canId = 50;
 
-      ClimberArmControls.Constants.autoZeroSettings.voltage = -1;
-      // Set this to something big, we are never going to use stall current to detect if climber has
-      // reached it's end of range of motion.
-      ClimberArmControls.Constants.autoZeroSettings.minResetCurrent = 0.5;
-      ClimberArmControls.Constants.autoZeroSettings.resetPositionRad =
-          simpleMotorSettings.minPositionInRads;
-      ClimberArmControls.Constants.autoZeroSettings.initialReverseDuration = 0;
+    //   ClimberArmControls.Constants.autoZeroSettings.voltage = -1;
+    //   // Set this to something big, we are never going to use stall current to detect if climber has
+    //   // reached it's end of range of motion.
+    //   ClimberArmControls.Constants.autoZeroSettings.minResetCurrent = 0.5;
+    //   ClimberArmControls.Constants.autoZeroSettings.resetPositionRad =
+    //       simpleMotorSettings.minPositionInRads;
+    //   ClimberArmControls.Constants.autoZeroSettings.initialReverseDuration = 0;
 
-      climberArm =
-          new SimpleMotorSubsystem(
-              new MotorIOTalonFx(motorSettings, settings), "Climber", simpleMotorSettings);
-    }
+    //   climberArm =
+    //       new SimpleMotorSubsystem(
+    //           new MotorIOTalonFx(motorSettings, settings), "Climber", simpleMotorSettings);
+    // }
 
     // LED
     {
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index 03df051a..bef4a151 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -1,7 +1,7 @@
 {
     "fileName": "AdvantageKit.json",
     "name": "AdvantageKit",
-    "version": "4.1.0",
+    "version": "4.1.2",
     "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-java",
-            "version": "4.1.0"
+            "version": "4.1.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-wpilibio",
-            "version": "4.1.0",
+            "version": "4.1.2",
             "skipInvalidPlatforms": false,
             "isJar": false,
             "validPlatforms": [
diff --git a/vendordeps/PathplannerLib-2025.2.4.json b/vendordeps/PathplannerLib-2025.2.7.json
similarity index 87%
rename from vendordeps/PathplannerLib-2025.2.4.json
rename to vendordeps/PathplannerLib-2025.2.7.json
index 24add570..d3f84e53 100644
--- a/vendordeps/PathplannerLib-2025.2.4.json
+++ b/vendordeps/PathplannerLib-2025.2.7.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "PathplannerLib-2025.2.4.json",
+    "fileName": "PathplannerLib-2025.2.7.json",
     "name": "PathplannerLib",
-    "version": "2025.2.4",
+    "version": "2025.2.7",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.2.4"
+            "version": "2025.2.7"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.2.4",
+            "version": "2025.2.7",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
diff --git a/vendordeps/Phoenix6-25.3.1.json b/vendordeps/Phoenix6-frc2025-latest.json
similarity index 86%
rename from vendordeps/Phoenix6-25.3.1.json
rename to vendordeps/Phoenix6-frc2025-latest.json
index a216d975..6f40c840 100644
--- a/vendordeps/Phoenix6-25.3.1.json
+++ b/vendordeps/Phoenix6-frc2025-latest.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.3.1.json",
+    "fileName": "Phoenix6-frc2025-latest.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.3.1",
+    "version": "25.4.0",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.3.1"
+            "version": "25.4.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +152,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -194,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -208,7 +208,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdi",
-            "version": "25.3.1",
+            "version": "25.4.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdle",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -224,7 +238,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -240,7 +254,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -256,7 +270,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -272,7 +286,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -288,7 +302,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -304,7 +318,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -320,7 +334,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -336,7 +350,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -352,7 +366,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -368,7 +382,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProTalonFXS",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -384,7 +398,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -400,7 +414,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -416,7 +430,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -432,7 +446,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdi",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANdi",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -444,6 +458,22 @@
                 "osxuniversal"
             ],
             "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdle",
+            "version": "25.4.0",
+            "libName": "CTRE_SimProCANdle",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
         }
     ]
 }
\ No newline at end of file
diff --git a/vendordeps/REVLib-2025.0.2.json b/vendordeps/REVLib.json
similarity index 90%
rename from vendordeps/REVLib-2025.0.2.json
rename to vendordeps/REVLib.json
index c29aefa1..ac62be88 100644
--- a/vendordeps/REVLib-2025.0.2.json
+++ b/vendordeps/REVLib.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib-2025.0.2.json",
+    "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2025.0.2",
+    "version": "2025.0.3",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.2"
+            "version": "2025.0.3"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.2",
+            "version": "2025.0.3",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -36,7 +36,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.2",
+            "version": "2025.0.3",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -53,7 +53,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.2",
+            "version": "2025.0.3",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json
index 00d3f73f..61a3f5da 100644
--- a/vendordeps/ThriftyLib.json
+++ b/vendordeps/ThriftyLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "ThriftyLib.json",
     "name": "ThriftyLib",
-    "version": "2025.0.2",
+    "version": "2025.1.1",
     "frcYear": "2025",
     "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.thethriftybot.frc",
             "artifactId": "ThriftyLib-java",
-            "version": "2025.0.2"
+            "version": "2025.1.1"
         }
     ],
     "jniDependencies": [],
diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json
index 579c2609..264dc8f5 100644
--- a/vendordeps/maple-sim.json
+++ b/vendordeps/maple-sim.json
@@ -1,7 +1,7 @@
 {
     "fileName": "maple-sim.json",
     "name": "maplesim",
-    "version": "0.3.8",
+    "version": "0.3.13",
     "frcYear": "2025",
     "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b",
     "mavenUrls": [
@@ -13,7 +13,7 @@
         {
             "groupId": "org.ironmaple",
             "artifactId": "maplesim-java",
-            "version": "0.3.8"
+            "version": "0.3.13"
         },
         {
             "groupId": "org.dyn4j",
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index 6af3d3e2..2d7b1d8e 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "photonlib.json",
     "name": "photonlib",
-    "version": "v2025.1.1",
+    "version": "v2025.3.1",
     "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
     "frcYear": "2025",
     "mavenUrls": [
@@ -13,7 +13,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-cpp",
-            "version": "v2025.1.1",
+            "version": "v2025.3.1",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -28,7 +28,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photonlib-cpp",
-            "version": "v2025.1.1",
+            "version": "v2025.3.1",
             "libName": "photonlib",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -43,7 +43,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-cpp",
-            "version": "v2025.1.1",
+            "version": "v2025.3.1",
             "libName": "photontargeting",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -60,12 +60,12 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photonlib-java",
-            "version": "v2025.1.1"
+            "version": "v2025.3.1"
         },
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-java",
-            "version": "v2025.1.1"
+            "version": "v2025.3.1"
         }
     ]
 }
\ No newline at end of file

From 142740840fdcddc7b8915e0946e75587340ab515 Mon Sep 17 00:00:00 2001
From: Ethan Porter <emanporter2009@gmail.com>
Date: Wed, 29 Oct 2025 19:19:59 -0400
Subject: [PATCH 90/90] spotless

---
 .../robot/config/game/reefscape2025/RobotConfigComp.java   | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index cd8b6779..443ac55c 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -6,7 +6,6 @@
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -22,18 +21,15 @@
 import frc.robot.io.implementations.motor.MotorIOBase.MotorIOBaseSettings;
 import frc.robot.io.implementations.motor.MotorIOTalonFx;
 import frc.robot.io.implementations.motor.MotorIOTalonFx.TalonFxSettings;
-import frc.robot.subsystems.controls.arm.ClimberArmControls;
 import frc.robot.subsystems.controls.arm.CoralArmControls;
 import frc.robot.subsystems.controls.elevator.ElevatorControls;
 import frc.robot.subsystems.implementations.drive.DriveBase;
 import frc.robot.subsystems.implementations.drive.DriveSwerveYAGSL;
 import frc.robot.subsystems.implementations.motor.ArmMotorSubsystem;
 import frc.robot.subsystems.implementations.motor.ElevatorMotorSubsystem;
-import frc.robot.subsystems.implementations.motor.SimpleMotorSubsystem;
 import frc.robot.subsystems.interfaces.Arm.ArmSettings;
 import frc.robot.subsystems.interfaces.Drive;
 import frc.robot.subsystems.interfaces.Elevator.ElevatorSettings;
-import frc.robot.subsystems.interfaces.SimpleMotor.SimpleMotorSettings;
 import frc.robot.subsystems.interfaces.Vision.Camera;
 
 public class RobotConfigComp extends RobotConfig {
@@ -210,7 +206,8 @@ public RobotConfigComp() {
     //   settings.canId = 50;
 
     //   ClimberArmControls.Constants.autoZeroSettings.voltage = -1;
-    //   // Set this to something big, we are never going to use stall current to detect if climber has
+    //   // Set this to something big, we are never going to use stall current to detect if climber
+    // has
     //   // reached it's end of range of motion.
     //   ClimberArmControls.Constants.autoZeroSettings.minResetCurrent = 0.5;
     //   ClimberArmControls.Constants.autoZeroSettings.resetPositionRad =