Fix Unit Problem and Align code with AK2025 TalonFX Template

src/main/deploy/pathplanner/paths/Consistancy Test.path

@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 9.29,
-        "y": 6.22
+        "x": 9.141987426415538,
+        "y": 6.318675049056306
       },
       "isLocked": false,
       "linkedName": null
@@ -33,7 +33,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.0,
+    "maxVelocity": 0.5,
     "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,

src/main/java/frc/robot/Constants.java

@@ -19,17 +19,21 @@
 
 import com.fasterxml.jackson.core.JsonProcessingException;
 import com.fasterxml.jackson.databind.ObjectMapper;
+import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Rotation2d;
 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.Filesystem;
 import edu.wpi.first.wpilibj.RobotBase;
 import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType;
+import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.SwerveConstants;
 import frc.robot.util.Alert;
 import frc.robot.util.Alert.AlertType;
@@ -135,10 +139,6 @@ 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
-    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);
-
     // Hold time on motor brakes when disabled
     public static final double kWheelLockTime = 10; // seconds
 
@@ -235,10 +235,30 @@ public static class OperatorConstants {
   /** Autonomous Action Constants ****************************************** */
   public static final class AutoConstants {
 
-    // PathPlanner Translation PID constants
-    public static final PIDConstants kAutoDrivePID = new PIDConstants(0.7, 0, 0);
-    // PathPlanner Rotation PID constants
-    public static final PIDConstants kAutoSteerPID = new PIDConstants(0.4, 0, 0.01);
+    // Drive and Turn PID constants used for PathPlanner
+    public static final PIDConstants kPPdrivePID = new PIDConstants(5.0, 0.0, 0.0);
+    public static final PIDConstants kPPsteerPID = new PIDConstants(5.0, 0.0, 0.0);
+
+    // PathPlanner Config constants
+    public static final RobotConfig kPathPlannerConfig =
+        new RobotConfig(
+            PhysicalConstants.kRobotMassKg,
+            PhysicalConstants.kRobotMOI,
+            new ModuleConfig(
+                SwerveConstants.kWheelRadiusMeters,
+                DrivebaseConstants.kMaxLinearSpeed,
+                PhysicalConstants.kWheelCOF,
+                DCMotor.getKrakenX60Foc(1).withReduction(SwerveConstants.kDriveGearRatio),
+                SwerveConstants.kDriveSlipCurrent,
+                1),
+            Drive.getModuleTranslations());
+
+    // Alternatively, we can build this from the PathPlanner GUI:
+    // public static final RobotConfig kPathPlannerConfig = RobotConfig.fromGUISettings();
+
+    // Drive and Turn PID constants used for Chorep
+    public static final PIDConstants kChoreoDrivePID = new PIDConstants(10.0, 0.0, 0.0);
+    public static final PIDConstants kChoreoSteerPID = new PIDConstants(7.5, 0.0, 0.0);
   }
 
   /** Vision Constants (Assuming PhotonVision) ***************************** */

src/main/java/frc/robot/Robot.java

@@ -40,8 +40,7 @@ public class Robot extends LoggedRobot {
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
    */
-  @Override
-  public void robotInit() {
+  public Robot() {
     // Record metadata
     Logger.recordMetadata("Robot", Constants.getRobot().toString());
     Logger.recordMetadata("TuningMode", Boolean.toString(Constants.tuningMode));
@@ -85,18 +84,14 @@ public void robotInit() {
         break;
     }
 
-    // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit.
-    // Logger.disableDeterministicTimestamps()
-
     // Start AdvantageKit logger
     Logger.start();
 
-    // Instantiate our RobotContainer. This will perform all our button bindings,
-    // and put our autonomous chooser on the dashboard.
+    // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
 
-    // Create a timer to disable motor brake a few seconds after disable. This will
-    // let the robot
+    // Create a timer to disable motor brake a few seconds after disable. This will let the robot
     // stop immediately when disabled, but then also let it be pushed more
     m_disabledTimer = new Timer();
   }
@@ -144,8 +139,11 @@ public void disabledPeriodic() {
   @Override
   public void autonomousInit() {
 
-    // TODO: Make sure Gyro inits here with whatever is in the path planning thingie
+    // Just in case, cancel all running commands
+    CommandScheduler.getInstance().cancelAll();
     m_robotContainer.setMotorBrake(true);
+
+    // TODO: Make sure Gyro inits here with whatever is in the path planning thingie
     switch (Constants.getAutoType()) {
       case PATHPLANNER:
         m_autoCommandPathPlanner = m_robotContainer.getAutonomousCommandPathPlanner();
@@ -179,7 +177,6 @@ public void teleopInit() {
     } else {
       CommandScheduler.getInstance().cancelAll();
     }
-    m_robotContainer.setDriveMode();
     m_robotContainer.setMotorBrake(true);
   }
 
@@ -192,7 +189,6 @@ public void teleopPeriodic() {}
   public void testInit() {
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
-    m_robotContainer.setDriveMode();
   }
 
   /** This function is called periodically during test mode. */

src/main/java/frc/robot/RobotContainer.java

@@ -26,7 +26,6 @@
 import choreo.auto.AutoRoutine;
 import choreo.auto.AutoTrajectory;
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -164,6 +163,7 @@ public RobotContainer() {
         autoChooserChoreo = null;
         autoFactoryChoreo = null;
         break;
+
       case CHOREO:
         autoFactoryChoreo =
             new AutoFactory(
@@ -179,24 +179,25 @@ public RobotContainer() {
         // Set the others to null
         autoChooserPathPlanner = null;
         break;
+
       default:
         // Then, throw the error
         throw new RuntimeException(
             "Incorrect AUTO type selected in Constants: " + Constants.getAutoType());
     }
 
-    // Configure the trigger bindings
-    configureBindings();
     // Define Auto commands
     defineAutoCommands();
     // Define SysIs Routines
     definesysIdRoutines();
+    // Configure the button and trigger bindings
+    configureBindings();
   }
 
   /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */
   private void defineAutoCommands() {
 
-    NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.zero()));
+    // NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.zero()));
   }
 
   /**
@@ -257,7 +258,7 @@ private void configureBindings() {
         .onTrue(
             Commands.runOnce(
                     () ->
-                        m_drivebase.setPose(
+                        m_drivebase.resetPose(
                             new Pose2d(m_drivebase.getPose().getTranslation(), new Rotation2d())),
                     m_drivebase)
                 .ignoringDisable(true));
@@ -295,10 +296,6 @@ public void getAutonomousCommandChoreo() {
     RobotModeTriggers.autonomous().whileTrue(autoChooserChoreo.selectedCommandScheduler());
   }
 
-  public void setDriveMode() {
-    configureBindings();
-  }
-
   /** Set the motor neutral mode to BRAKE / COAST for T/F */
   public void setMotorBrake(boolean brake) {
     m_drivebase.setMotorBrake(brake);

src/main/java/frc/robot/commands/ChoreoAutoController.java

@@ -27,11 +27,11 @@
 public class ChoreoAutoController implements Consumer<SwerveSample> {
   private final Drive drive; // drive subsystem
   private final PIDController xController =
-      new PIDController(kAutoDrivePID.kP, 0.0, kAutoDrivePID.kD);
+      new PIDController(kChoreoDrivePID.kP, kChoreoDrivePID.kI, kChoreoDrivePID.kD);
   private final PIDController yController =
-      new PIDController(kAutoDrivePID.kP, 0.0, kAutoDrivePID.kD);
+      new PIDController(kChoreoDrivePID.kP, kChoreoDrivePID.kI, kChoreoDrivePID.kD);
   private final PIDController headingController =
-      new PIDController(kAutoSteerPID.kP, 0.0, kAutoSteerPID.kD);
+      new PIDController(kChoreoSteerPID.kP, kChoreoSteerPID.kI, kChoreoSteerPID.kD);
 
   public ChoreoAutoController(Drive drive) {
     this.drive = drive;

src/main/java/frc/robot/commands/DriveCommands.java

@@ -30,6 +30,7 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.AutoConstants;
 import frc.robot.Constants.DrivebaseConstants;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.subsystems.drive.Drive;
@@ -125,9 +126,9 @@ public static Command fieldRelativeDriveAtAngle(
     // Create PID controller
     ProfiledPIDController angleController =
         new ProfiledPIDController(
-            DrivebaseConstants.steerPID.kP,
-            DrivebaseConstants.steerPID.kI,
-            DrivebaseConstants.steerPID.kD,
+            AutoConstants.kPPsteerPID.kP,
+            AutoConstants.kPPsteerPID.kI,
+            AutoConstants.kPPsteerPID.kD,
             new TrapezoidProfile.Constraints(
                 DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel));
     angleController.enableContinuousInput(-Math.PI, Math.PI);

src/main/java/frc/robot/generated/TunerConstants.java

@@ -138,7 +138,7 @@ public class TunerConstants {
   private static final int kFrontLeftDriveMotorId = 1;
   private static final int kFrontLeftSteerMotorId = 2;
   private static final int kFrontLeftEncoderId = 3;
-  private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.087158203125);
+  private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.088134765625);
   private static final boolean kFrontLeftSteerMotorInverted = true;
   private static final boolean kFrontLeftEncoderInverted = false;
 
@@ -149,7 +149,7 @@ public class TunerConstants {
   private static final int kFrontRightDriveMotorId = 4;
   private static final int kFrontRightSteerMotorId = 5;
   private static final int kFrontRightEncoderId = 6;
-  private static final Angle kFrontRightEncoderOffset = Rotations.of(0.410888671875);
+  private static final Angle kFrontRightEncoderOffset = Rotations.of(0.23779296875);
   private static final boolean kFrontRightSteerMotorInverted = true;
   private static final boolean kFrontRightEncoderInverted = false;
 
@@ -160,7 +160,7 @@ public class TunerConstants {
   private static final int kBackLeftDriveMotorId = 7;
   private static final int kBackLeftSteerMotorId = 8;
   private static final int kBackLeftEncoderId = 9;
-  private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.1142578125);
+  private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.114990234375);
   private static final boolean kBackLeftSteerMotorInverted = true;
   private static final boolean kBackLeftEncoderInverted = false;
 
@@ -171,7 +171,7 @@ public class TunerConstants {
   private static final int kBackRightDriveMotorId = 10;
   private static final int kBackRightSteerMotorId = 11;
   private static final int kBackRightEncoderId = 12;
-  private static final Angle kBackRightEncoderOffset = Rotations.of(-0.471923828125);
+  private static final Angle kBackRightEncoderOffset = Rotations.of(-0.47265625);
   private static final boolean kBackRightSteerMotorInverted = true;
   private static final boolean kBackRightEncoderInverted = false;