Add a command cancel in autoInit()

Make sure all PathPlanner PIDs point to values in `Constants.java`. modified: src/main/java/frc/robot/Constants.java modified: src/main/java/frc/robot/Robot.java modified: src/main/java/frc/robot/subsystems/drive/Drive.java
AZ-First · Jan 15, 2025 · eddf329 · eddf329
1 parent e52c1d9
commit eddf329
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -135,7 +135,7 @@ public static final class DrivebaseConstants {
     public static final double kMaxLinearAccel = 4.0; // m/s/s
     public static final double kMaxAngularAccel = Units.degreesToRadians(720);
 
-    // Drive and Turn PID constants
+    // Drive and Turn PID constants used for PathPlanner
     public static final PIDConstants drivePID = new PIDConstants(0.05, 0.0, 0.0);
     public static final PIDConstants steerPID = new PIDConstants(2.0, 0.0, 0.4);
 

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -144,6 +144,9 @@ public void disabledPeriodic() {
   @Override
   public void autonomousInit() {
 
+    // Just in case, cancel all running commands
+    CommandScheduler.getInstance().cancelAll();
+
     // TODO: Make sure Gyro inits here with whatever is in the path planning thingie
     m_robotContainer.setMotorBrake(true);
     switch (Constants.getAutoType()) {

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -22,7 +22,6 @@
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.config.ModuleConfig;
-import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.pathfinding.Pathfinding;
@@ -97,9 +96,6 @@ public class Drive extends SubsystemBase {
   private SwerveDrivePoseEstimator m_PoseEstimator =
       new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
 
-  // Choreo drive controller
-  private final PIDController xController = new PIDController(10.0, 0.0, 0.0);
-
   // Constructor
   public Drive() {
     switch (Constants.getSwerveType()) {
@@ -169,7 +165,7 @@ public Drive() {
             this::getChassisSpeeds,
             this::runVelocity,
             new PPHolonomicDriveController(
-                new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)),
+                DrivebaseConstants.drivePID, DrivebaseConstants.steerPID),
             PP_CONFIG,
             () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
             this);
@@ -441,6 +437,7 @@ public Command resetOdometry(Pose2d orElseGet) {
   private final SwerveRequest.ApplyFieldSpeeds m_pathApplyFieldSpeeds =
       new SwerveRequest.ApplyFieldSpeeds();
 
+  // Choreo Controller Values
   private final PIDController m_pathXController = new PIDController(10, 0, 0);
   private final PIDController m_pathYController = new PIDController(10, 0, 0);
   private final PIDController m_pathThetaController = new PIDController(7, 0, 0);
@@ -474,9 +471,10 @@ public void followTrajectory(SwerveSample sample) {
     // Generate the next speeds for the robot
     ChassisSpeeds speeds =
         new ChassisSpeeds(
-            sample.vx + xController.calculate(pose.getX(), sample.x),
-            sample.vy + xController.calculate(pose.getX(), sample.y),
-            sample.omega + xController.calculate(pose.getRotation().getRadians(), sample.heading));
+            sample.vx + m_pathXController.calculate(pose.getX(), sample.x),
+            sample.vy + m_pathXController.calculate(pose.getX(), sample.y),
+            sample.omega
+                + m_pathXController.calculate(pose.getRotation().getRadians(), sample.heading));
 
     // Apply the generated speeds
     runVelocity(speeds);