Merge pull request #22 from Team4388/Debug-Autos

Debug autos

src/main/deploy/pathplanner/autos/Stupid auto.auto

@@ -0,0 +1,31 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Stupid auto 1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Stupid auto 2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Stupid auto 3"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Test Auto.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Stupid auto 1"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path

@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -122.12499844038754
+    "rotation": 60.35013649242443
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 178.97696981133208
+    "rotation": 0.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path

@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 59.82647997035557
+    "rotation": -121.27770286686648
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 54.32359177813805
+    "rotation": -126.57303097851931
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path

@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 121.75948008481289
+    "rotation": -57.4259428654274
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 179.30130561701642
+    "rotation": -0.9548412538721401
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path

@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 54.0394828033551
+    "rotation": -123.50343698241421
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 119.16761337957772
+    "rotation": -59.036243467926624
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Center Barge to Reef.path

@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 179.65844947344297
+    "rotation": 0.5456575934157175
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 0.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 5.197222222222222,
+        "x": 5.268371212121212,
-        "y": 5.52929292929293
+        "y": 5.417487373737374
       },
       "prevControl": {
-        "x": 6.234912180385907,
+        "x": 6.306061170284897,
-        "y": 5.781923450986739
+        "y": 5.670117895431184
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -121.60750224624891
+    "rotation": 60.851928154286895
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 179.44374777291927
+    "rotation": 1.1457628387483283
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path

@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 118.92642583525368
+    "rotation": -62.35402463626126
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 1.97493401088202
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path

@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 50.964487101253106
+    "rotation": -124.91183021661
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -178.99491399474581
+    "rotation": -1.6365770416167795
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Center Reef to Feed.path

@@ -25,7 +25,7 @@
       },
       "nextControl": {
         "x": 3.388005050505051,
-        "y": 6.11881313131313
+        "y": 6.1188131313131295
       },
       "isLocked": false,
       "linkedName": null
@@ -58,13 +58,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -52.30575953331084
+    "rotation": 126.3843518158359
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -177.95459151111288
+    "rotation": 0.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Stupid auto 1.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.081502525252525,
+        "y": 7.267361111111111
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.871969696969696,
+        "y": 8.070328282828283
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.081502525252525,
+        "y": 5.10239898989899
+      },
+      "prevControl": {
+        "x": 4.821148989898989,
+        "y": 5.10239898989899
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Stupid auto 2.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.101830808080808,
+        "y": 5.041414141414141
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.10183080808081,
+        "y": 5.041414141414141
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.585795454545455,
+        "y": 6.149305555555555
+      },
+      "prevControl": {
+        "x": 8.175315656565658,
+        "y": 5.295517676767678
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Stupid auto 3.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.585795454545455,
+        "y": 6.200126262626262
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.778914141414141,
+        "y": 7.033585858585859
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.081502525252525,
+        "y": 7.32834595959596
+      },
+      "prevControl": {
+        "x": 7.595959595959596,
+        "y": 7.724747474747475
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Taxi.path

@@ -23,6 +23,22 @@
         "x": 5.928529040722133,
         "y": 7.353679406601922
       },
+      "nextControl": {
+        "x": 6.681698232005139,
+        "y": 7.343669078246562
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.8889204545454543,
+        "y": 7.348674242424242
+      },
+      "prevControl": {
+        "x": 4.138853417354843,
+        "y": 7.3544633789160185
+      },
       "nextControl": null,
       "isLocked": false,
       "linkedName": null
@@ -42,13 +58,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 179.47479945510887
+    "rotation": -1.9251837083231407
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 177.6140559696112
+    "rotation": -1.1306909512379957
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path

@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -120.0183674276091
+    "rotation": 60.255118703057796
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 179.30972280213487
+    "rotation": -2.223961240385466
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Top Feed to Reef.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.263699494949495,
+        "x": 1.0807449494949497,
-        "y": 7.135227272727272
+        "y": 6.972601010101011
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.6862557056350642,
+        "x": 2.0869949494949496,
-        "y": 6.8478211564289
+        "y": 6.291603535353536
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.6217803030303033,
+        "x": 3.764078282828282,
-        "y": 5.417487373737374
+        "y": 5.24469696969697
       },
       "prevControl": {
-        "x": 3.111746968518841,
+        "x": 2.9836492542425197,
-        "y": 5.776730048021274
+        "y": 5.685899280282051
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -60.228308784269046
+    "rotation": 121.34521414171566
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -55.124671655397854
+    "rotation": 124.23611780915067
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path

@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 120.25643716352924
+    "rotation": -54.659893078442295
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -179.06080905426438
+    "rotation": 0.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Top Reef to Feed.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.192550505050505,
+        "x": 1.2230429292929292,
-        "y": 7.0234217171717175
+        "y": 7.003093434343434
       },
       "prevControl": {
-        "x": 0.9300747161944047,
+        "x": 0.960567140436829,
-        "y": 7.106516384157039
+        "y": 7.086188101328755
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -52.56142842766699
+    "rotation": 126.86989764584399
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -121.75948008481281
+    "rotation": 57.21571913413089
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/settings.json

@@ -9,7 +9,7 @@
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 22.6796,
+  "robotMass": 74.088,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.048,

src/main/java/frc4388/robot/Constants.java

@@ -230,7 +230,7 @@ public final class Constants {
         }
     
         public static final class AutoConstants {
-            public static final Gains XY_GAINS = new Gains(3,0,0);
+            public static final Gains XY_GAINS = new Gains(3,0.01,0);
             public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
 
             public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0);
@@ -240,7 +240,7 @@ public final class Constants {
             public static final int LIDAR_MICROS_TO_CM = 10;
             public static final int SECONDS_TO_MICROS = 1000000;
                     
-            public static final double XY_TOLERANCE = 0.05;
+            public static final double XY_TOLERANCE = 0.1;
             public static final double ROT_TOLERANCE = 1;
                     
             // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
@@ -249,8 +249,8 @@ public final class Constants {
     
     
         public static final class Configurations {
-            public static final double OPEN_LOOP_RAMP_RATE = 0.0; // Todo: Test. think this will help.
+            public static final double OPEN_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help.
-            public static final double CLOSED_LOOP_RAMP_RATE = 0.0; // Todo: Test. think this will help.
+            public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help.
             public static final double NEUTRAL_DEADBAND = 0.04;
 
             // POWER! (limiting)
@@ -354,7 +354,8 @@ public final class Constants {
 
     public static final class FieldConstants {
         public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
-        public static final double HORISONTAL_SCORING_POSITION_OFFSET = 0;
+        public static final double HORISONTAL_SCORING_POSITION_OFFSET = .2794
+        ;
         // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
 
         public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20);

src/main/java/frc4388/robot/Robot.java

@@ -17,6 +17,7 @@ import com.ctre.phoenix6.CANBus.CANBusStatus;
 
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -85,6 +86,7 @@ public class Robot extends TimedRobot {
   @Override
   public void robotPeriodic() {   
     m_robotTime.updateTimes();
+    SmartDashboard.putNumber("Time", System.currentTimeMillis());
     //System.out.println(m_robotContainer.limelight.isNearSpeaker());
     //mled.updateLED();
     // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
@@ -143,13 +145,20 @@ public class Robot extends TimedRobot {
 
   @Override
   public void teleopInit() {
+    m_robotContainer.stop();
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
+
     if (m_autonomousCommand != null) {
       CommandScheduler.getInstance().cancel(m_autonomousCommand);
       m_autonomousCommand.cancel();
+      m_autonomousCommand.end(true);
+      System.out.println("NOT Null!!");
+
+    } else {
+      System.out.println("Null!!");
     }
     m_robotTime.startMatchTime();
   }

src/main/java/frc4388/robot/RobotContainer.java

@@ -92,6 +92,10 @@ public class RobotContainer {
     // public List<Subsystem> subsystems = new ArrayList<>();
 
     // ! Teleop Commands
+    public void stop() {
+        new InstantCommand(()->{}, m_robotSwerveDrive).schedule();;;;;
+        m_robotSwerveDrive.stopModules();
+    }
 
     // ! /*  Autos */
     private String lastAutoName = "defualt.auto";
@@ -104,8 +108,10 @@ public class RobotContainer {
     private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision);
 
     private Command placeCoral = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotSwerveDrive.stopModules()),
         new InstantCommand(() -> System.out.println("Placing Some Coral")),
-        new WaitCommand(3)
+        new WaitCommand(3),
+        new InstantCommand(() -> System.out.println("Done placing Some Coral"))
     );
 
     private Command aprilAlign = new SequentialCommandGroup(
@@ -115,7 +121,8 @@ public class RobotContainer {
 
     private Command grabCoral = new SequentialCommandGroup(
         new InstantCommand(() -> System.out.println("Yoinking some coral...")),
-        new WaitCommand(2)
+        new WaitCommand(2),
+        new InstantCommand(() -> System.out.println("Done yoinking some coral..."))
     );
     
     /**
@@ -124,7 +131,7 @@ public class RobotContainer {
     public RobotContainer() {
         
         NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition);
-        NamedCommands.registerCommand("april-allign", aprilAlign);
+        NamedCommands.registerCommand("april-allign", AutoGotoPosition);
         NamedCommands.registerCommand("place-coral", placeCoral);
         NamedCommands.registerCommand("grab-coral", grabCoral);
 
@@ -210,7 +217,7 @@ public class RobotContainer {
         // ?  /* Operator Buttons */
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-            .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision));
+            .onTrue(AutoGotoPosition);
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
             .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); 
@@ -282,10 +289,10 @@ public class RobotContainer {
 
         //return autoPlayback;
         //return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
-        return autoChooser.getSelected();
+        //return autoChooser.getSelected();
 	// try{
 	//     // Load the path you want to follow using its name in the GUI
-	//     return new PathPlannerAuto("New Auto");
+	    return new PathPlannerAuto("Red Center Cage");
 	// } catch (Exception e) {
 	//     DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
 	//     return Commands.none();

src/main/java/frc4388/robot/commands/GotoPositionCommand.java

@@ -37,14 +37,14 @@ public class GotoPositionCommand extends Command {
     public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
         this.swerveDrive = swerveDrive;
         this.vision = vision;
-        addRequirements(swerveDrive);
+        // addRequirements(swerveDrive);
     }
 
     @Override
     public void initialize() {
         xPID.initialize();
         yPID.initialize();
-        this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.RIGHT, AutoConstants.X_OFFSET_TRIM.get());
+        this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.LEFT, AutoConstants.X_OFFSET_TRIM.get());
     }
     
     double xerr;
@@ -84,7 +84,9 @@ public class GotoPositionCommand extends Command {
 
     @Override
     public final boolean isFinished() {
-        return (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
+        boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
+        // System.out.println(finished);
+        return finished;
                 // this statement is a boolean in and of itself
     }