auton vision stuff changes

Eddie-Edward · Eddie-Edward · commit fb191bc6e46b · 2025-04-04T19:18:04.000-04:00
diff --git a/src/main/deploy/pathplanner/paths/F_Load.path b/src/main/deploy/pathplanner/paths/F_Load.path
@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.2692826086956535,
-        "y": 1.5998260869565217
+        "x": 4.830908026069234,
+        "y": 1.9518060929976624
       },
       "isLocked": false,
       "linkedName": "F"
@@ -20,8 +20,8 @@
         "y": 0.7151315789473688
       },
       "prevControl": {
-        "x": 2.566518878718536,
-        "y": 1.3681207093821506
+        "x": 3.069619172428834,
+        "y": 1.3063477775858439
       },
       "nextControl": null,
       "isLocked": false,
@@ -40,12 +40,12 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 3.5,
+    "maxVelocity": 4.75,
+    "maxAcceleration": 4.25,
     "maxAngularVelocity": 400.0,
     "maxAngularAcceleration": 600.0,
     "nominalVoltage": 12.0,
-    "unlimited": true
+    "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
@@ -57,5 +57,5 @@
     "velocity": 0,
     "rotation": -59.82647997035571
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
diff --git a/src/main/deploy/pathplanner/paths/G_GH-Algae.path b/src/main/deploy/pathplanner/paths/G_GH-Algae.path
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.63,
+        "x": 6.2692499999999995,
         "y": 3.966499999999999
       },
       "prevControl": null,
       "nextControl": {
-        "x": 8.12673913043478,
+        "x": 7.76598913043478,
         "y": 3.87111956521739
       },
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/G_LGr.path b/src/main/deploy/pathplanner/paths/G_LGr.path
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.63,
+        "x": 6.2692499999999995,
         "y": 3.966499999999999
       },
       "prevControl": null,
       "nextControl": {
-        "x": 8.535065217391303,
+        "x": 8.174315217391305,
         "y": 5.6906847826086935
       },
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/G_RGr.path b/src/main/deploy/pathplanner/paths/G_RGr.path
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.63,
+        "x": 6.2692499999999995,
         "y": 3.966499999999999
       },
       "prevControl": null,
       "nextControl": {
-        "x": 8.153543478260868,
+        "x": 7.792793478260868,
         "y": 2.491771739130434
       },
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/I_Load.path b/src/main/deploy/pathplanner/paths/I_Load.path
@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.222847826086958,
-        "y": 6.526152173913043
+        "x": 4.921869781764967,
+        "y": 6.00718707747607
       },
       "isLocked": false,
       "linkedName": "I"
@@ -20,8 +20,8 @@
         "y": 7.3541118421052625
       },
       "prevControl": {
-        "x": 3.0374057591185633,
-        "y": 6.950586503581804
+        "x": 3.226745198966955,
+        "y": 6.6621220020961385
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,12 +33,12 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 3.5,
+    "maxVelocity": 4.75,
+    "maxAcceleration": 4.25,
     "maxAngularVelocity": 400.0,
     "maxAngularAcceleration": 600.0,
     "nominalVoltage": 12.0,
-    "unlimited": true
+    "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 59.966719564004734
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
diff --git a/src/main/deploy/pathplanner/paths/Start_G.path b/src/main/deploy/pathplanner/paths/Start_G.path
@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 6.63,
+        "x": 6.2692499999999995,
         "y": 3.966499999999999
       },
       "prevControl": {
-        "x": 7.323755078679089,
+        "x": 6.963005078679089,
         "y": 3.963511937221352
       },
       "nextControl": null,
diff --git a/src/main/deploy/pathplanner/paths/Start_Mid_1.path b/src/main/deploy/pathplanner/paths/Start_Mid_1.path
@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 6.63,
+        "x": 6.2692499999999995,
         "y": 3.966499999999999
       },
       "prevControl": {
-        "x": 7.323755078679089,
+        "x": 6.963005078679089,
         "y": 3.963511937221352
       },
       "nextControl": null,
@@ -33,10 +33,10 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 3.5,
-    "maxAngularVelocity": 400.0,
-    "maxAngularAcceleration": 600.0,
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 200.0,
+    "maxAngularAcceleration": 300.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 0.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -158,15 +158,15 @@ public void setDefaultCommands() {
         //         new ElevatorSubsystem.DefaultCommand(ElevatorSubsystem.getInstance(), operator::getElevatorOutput)
         // );
 
-        // ElevatorSubsystem.getInstance().setDefaultCommand(
-        //     new ElevatorSubsystem.zero()
-        // );
-        AlgaeSubsystem.getInstance().setDefaultCommand(
-            new AlgaeSubsystem.DefaultCommand()
-        );
         RushinatorPivot.getInstance().setDefaultCommand(
             new RushinatorPivot.DefaultCommand()
         );
+        ElevatorSubsystem.getInstance().setDefaultCommand(
+            new ElevatorSubsystem.zero()
+        );
+        AlgaeSubsystem.getInstance().setDefaultCommand(
+            new AlgaeSubsystem.DefaultCommand()
+        );
         RushinatorRollers.getInstance().setDefaultCommand(
             new RushinatorRollers.DefaultCommand()
         );
diff --git a/src/main/java/frc/robot/auton/AutonMaster.java b/src/main/java/frc/robot/auton/AutonMaster.java
@@ -56,7 +56,7 @@ public AutonMaster() {
         try {
             var config = RobotConfig.fromGUISettings();
             AutoBuilder.configure(
-                drivetrain::getPose,   // Supplier of current robot pose
+                () -> PoseEstimatorSubsystem.getInstance().getCurrentPose(),   // Supplier of current robot pose
                 drivetrain::resetPose,         // Consumer for seeding pose against auto
                 drivetrain::getRobotRelvativeSpeeds, // Supplier of current robot speeds
                 // Consumer of ChassisSpeeds and feedforwards to drive the robot
diff --git a/src/main/java/frc/robot/commands/RobotCommands.java b/src/main/java/frc/robot/commands/RobotCommands.java
@@ -109,9 +109,9 @@ public static Command scoreCoralAutonL4(){
         return new SequentialCommandGroup(
             new ParallelRaceGroup(
                 new SetElevatorState(ElevatorSubsystem.State.kCoralScoreL4),
-                new SetArmState(RushinatorPivot.State.kScoreL4Auton),
+                new SetArmState(RushinatorPivot.State.kScoreL4),
                 new SetWristState(RushinatorWrist.State.kScoreL4RightWrist),
-                new WaitCommand(0.35)
+                new WaitCommand(0.4)
             ),
             new ParallelRaceGroup(
                 new SetArmState(RushinatorPivot.State.kStowTravel),
diff --git a/src/main/java/frc/robot/driver/DriverXbox.java b/src/main/java/frc/robot/driver/DriverXbox.java
@@ -168,25 +168,25 @@ public void setupTeleopButtons() {
         
 
         // Score L4 Auto Align
-        controller.y().and(() -> OperatorXbox.getInstance().controller.x().and(leftTriggerOnly()).getAsBoolean()).whileTrue(new ConditionalCommand(
-            RobotCommands.coralPrimeShoot(RushinatorPivot.State.kScoreL4, RushinatorWrist.State.kScoreL4RightWrist), 
-            RobotCommands.coralPrimeShoot(RushinatorPivot.State.kScoreL4, RushinatorWrist.State.kScoreL4LeftWrist), 
-            () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight || 
-            RushinatorWrist.kLastState == RushinatorWrist.State.kScoreRightWrist || 
-            RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || 
-            RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist ||  
-            RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || 
-            RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right ||
-            RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid || 
-            RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid ||
-            RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid)
-        );
-        controller.y().and(() -> OperatorXbox.getInstance().controller.x().and(leftTriggerOnly()).getAsBoolean()).whileTrue(
-            new SetElevatorState(ElevatorSubsystem.State.kCoralScoreL4)
-        );
-        controller.y().and(() -> OperatorXbox.getInstance().controller.x().and(leftTriggerOnly()).getAsBoolean()).whileTrue(
-            new SetRollersVoltage(0.0)
-        );
+        // controller.y().and(() -> OperatorXbox.getInstance().controller.x().and(leftTriggerOnly()).getAsBoolean()).whileTrue(new ConditionalCommand(
+        //     RobotCommands.coralPrimeShoot(RushinatorPivot.State.kScoreL4, RushinatorWrist.State.kScoreL4RightWrist), 
+        //     RobotCommands.coralPrimeShoot(RushinatorPivot.State.kScoreL4, RushinatorWrist.State.kScoreL4LeftWrist), 
+        //     () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight || 
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kScoreRightWrist || 
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || 
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist ||  
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || 
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right ||
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid || 
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid ||
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid)
+        // );
+        // controller.y().and(() -> OperatorXbox.getInstance().controller.x().and(leftTriggerOnly()).getAsBoolean()).whileTrue(
+        //     new SetElevatorState(ElevatorSubsystem.State.kCoralScoreL4)
+        // );
+        // controller.y().and(() -> OperatorXbox.getInstance().controller.x().and(leftTriggerOnly()).getAsBoolean()).whileTrue(
+        //     new SetRollersVoltage(0.0)
+        // );
 
         // controller.y().and(() -> OperatorXbox.getInstance().controller.y().getAsBoolean()).whileFalse(
         //     new SetElevatorState(ElevatorSubsystem.State.kCoralL4)
diff --git a/src/main/java/frc/robot/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/elevator/ElevatorSubsystem.java
@@ -65,7 +65,7 @@ public static class Settings {
     public enum State {
         kCoralL1(0.0),
         kCoralL2(0.0),
-        kCoralScoreL2(6.34619140625),
+        kCoralScoreL2(6.3461914062 - 1.0),
         kCoralL3(12.54296875),
         kCoralScoreL3(15.7583007812),
         kCoralL4AutonScore(16.2),
diff --git a/src/main/java/frc/robot/operator/OperatorXbox.java b/src/main/java/frc/robot/operator/OperatorXbox.java
@@ -202,22 +202,22 @@ public void setupTeleopButtons() {
         );
 
         // Score Prime Auto ALign L4
-        controller.x().and(leftTriggerOnly()).onTrue(RobotCommands.coralPrime(
-            RushinatorPivot.State.kStowAutoAlignL4, ElevatorSubsystem.State.kCoralL4)
-        );
-        controller.x().and(leftTriggerOnly()).onTrue(new ConditionalCommand(
-            new SetWristState(RushinatorWrist.State.kTravelL4Right), 
-            new SetWristState(RushinatorWrist.State.kTravelL4Left), 
-            () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight ||
-            RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right ||
-            RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || 
-            RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || 
-            RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || 
-            RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid ||
-            RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid ||
-            RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid
-            )
-        );
+        // controller.x().and(leftTriggerOnly()).onTrue(RobotCommands.coralPrime(
+        //     RushinatorPivot.State.kStowAutoAlignL4, ElevatorSubsystem.State.kCoralL4)
+        // );
+        // controller.x().and(leftTriggerOnly()).onTrue(new ConditionalCommand(
+        //     new SetWristState(RushinatorWrist.State.kTravelL4Right), 
+        //     new SetWristState(RushinatorWrist.State.kTravelL4Left), 
+        //     () -> RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight ||
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right ||
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist || 
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist || 
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist || 
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid ||
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid ||
+        //     RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid
+        //     )
+        // );
 
         // Algae L3
         controller.y().and(leftTriggerOnly()).onTrue(new SetElevatorState(ElevatorSubsystem.State.kAlgaeL3));
diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java
@@ -116,14 +116,19 @@ public static class AlignmentConfig {
         public static final double THETA_TOLERANCE = 0.03;
 
         public static final Distance ALIGNMENT_TOLERANCE = Inches.of(0.5); //inches
-        public static final LinearVelocity MAX_ALIGN_TRANSLATION_VELOCITY = TunerConstants.kSpeedAt12Volts.div(2);
+        public static final LinearVelocity MAX_ALIGN_TRANSLATION_VELOCITY = TunerConstants.kSpeedAt12Volts.div(8);
         public static final LinearAcceleration MAX_ALIGN_TRANSLATION_ACCELERATION = MetersPerSecondPerSecond.of(6.0);
         public static final AngularVelocity MAX_ALIGN_ANGULAR_VELOCITY = RotationsPerSecond.of(1.25).times(0.75);
         public static final AngularAcceleration MAX_ALIGN_ANGULAR_ACCELERATION = RadiansPerSecondPerSecond.of(6.0 * Math.PI);
         
     }
 
-    private static final double acutalX = 5.675;
+    /*Red Alliance */
+    private static final double acutalX = 5.775;
+
+    /*Blue Alliance */
+    // private static final double acutalX = 5.1;
+
     private static final double x = 0.5 * acutalX;
     private static final double y = (Math.sqrt(3)/2) * acutalX;
 
diff --git a/src/main/java/frc/robot/vision/commands/AutoAlign.java b/src/main/java/frc/robot/vision/commands/AutoAlign.java
@@ -41,9 +41,15 @@ public class AutoAlign extends Command {
   private final Distance TRANSLATION_TOLERANCE = Inches.of(0.25);
   private final Angle THETA_TOLERANCE = Degrees.of(1.0);
 
+  /* Blue Alliance */
+  // private final double branchOffsetRightWrist = Units.inchesToMeters(6.469);
+  // private final Transform2d leftBranchTransformRightWrist = new Transform2d(0.0, -branchOffsetRightWrist - Units.inchesToMeters(3.75), Rotation2d.kZero);
+  // private final Transform2d rightBranchTransformRightWrist = new Transform2d(0.0, branchOffsetRightWrist - Units.inchesToMeters(1.00), Rotation2d.kZero);
+
+  /* Red Alliance */
   private final double branchOffsetRightWrist = Units.inchesToMeters(6.469);
   private final Transform2d leftBranchTransformRightWrist = new Transform2d(0.0, -branchOffsetRightWrist - Units.inchesToMeters(0.75), Rotation2d.kZero);
-  private final Transform2d rightBranchTransformRightWrist = new Transform2d(0.0, branchOffsetRightWrist - Units.inchesToMeters(0.75), Rotation2d.kZero);
+  private final Transform2d rightBranchTransformRightWrist = new Transform2d(0.0, branchOffsetRightWrist - Units.inchesToMeters(1.25), Rotation2d.kZero);
 
   private final double branchOffsetLeftWrist = Units.inchesToMeters(6.469);
   private final Transform2d leftBranchTransformLeftWrist = new Transform2d(0.0, -branchOffsetLeftWrist + Units.inchesToMeters(1.25) , Rotation2d.kZero);
@@ -116,7 +122,7 @@ else if(oldReefFace == ReefFace.BLU_REEF_GH) {
         return ReefFace.BLU_REEF_GH_L4;
       }
       else if(oldReefFace == ReefFace.BLU_REEF_IJ) {
-        return ReefFace.BLU_REEF_IJ_L4;
+        return ReefFace.BLU_REEF_IJ_L4; 
       }
       else if(oldReefFace == ReefFace.BLU_REEF_KL) {
         return ReefFace.BLU_REEF_KL_L4;
