Logging and Tuning Improvements

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

@@ -262,19 +262,27 @@ public static final class DrivebaseConstants {
     // of YOUR ROBOT, and replace the estimate here with your measured value!
     public static final double kMaxLinearSpeed = Feet.of(18).in(Meters);
 
-    // Set 3/4 of a rotation per second as the max angular velocity (radians/sec)
-    public static final double kMaxAngularSpeed = 1.5 * Math.PI;
+    // Slip Current -- the current draw when the wheels start to slip
+    // Measure this against a wall.  CHECK WITH THE CARPET AT AN ACTUAL EVENT!!!
+    public static final double kSlipCurrent = 20.0; // Amps
+
+    // Characterized Wheel Radius (using the "Drive Wheel Radius Characterization" auto routine)
+    public static final double kWheelRadiusMeters = Inches.of(2.000).in(Meters);
 
     // Maximum chassis accelerations desired for robot motion  -- metric / radians
     // TODO: Compute the maximum linear acceleration given the PHYSICS of the ROBOT!
     public static final double kMaxLinearAccel = 4.0; // m/s/s
-    public static final double kMaxAngularAccel = Degrees.of(720).in(Radians);
 
-    // For Profiled PID Motion, these are the rotational PID Constants:
-    // TODO: Need tuning!
-    public static final double kPTheta = 5.0;
-    public static final double kITheta = 0.0;
-    public static final double kDTheta = 0.0;
+    // For Profiled PID Motion -- NEED TUNING!
+    // Used in a variety of contexts, including PathPlanner and AutoPilot
+    // Chassis (not module) across-the-field strafing motion
+    public static final double kPStrafe = 5.0;
+    public static final double kIStrafe = 0.0;
+    public static final double kDStrafe = 0.0;
+    // Chassis (not module) solid-body rotation
+    public static final double kPSPin = 5.0;
+    public static final double kISPin = 0.0;
+    public static final double kDSpin = 0.0;
 
     // Hold time on motor brakes when disabled
     public static final double kWheelLockTime = 10; // seconds
@@ -326,17 +334,21 @@ public static final class FlywheelConstants {
 
     // MODE == REAL / REPLAY
     // Feedforward constants
-    public static final double kStaticGainReal = 0.1;
-    public static final double kVelocityGainReal = 0.05;
+    public static final double kSreal = 0.1;
+    public static final double kVreal = 0.05;
+    public static final double kAreal = 0.0;
     // Feedback (PID) constants
-    public static final PIDConstants pidReal = new PIDConstants(1.0, 0.0, 0.0);
+    public static final double kPreal = 1.0;
+    public static final double kDreal = 0.0;
 
     // MODE == SIM
     // Feedforward constants
-    public static final double kStaticGainSim = 0.0;
-    public static final double kVelocityGainSim = 0.03;
+    public static final double kSsim = 0.0;
+    public static final double kVsim = 0.03;
+    public static final double kAsim = 0.0;
     // Feedback (PID) constants
-    public static final PIDConstants pidSim = new PIDConstants(1.0, 0.0, 0.0);
+    public static final double kPsim = 0.0;
+    public static final double kDsim = 0.0;
   }
 
   /************************************************************************* */
@@ -350,21 +362,17 @@ public static final class FlywheelConstants {
   public static final class AutoConstants {
 
     // ********** PATHPLANNER CONSTANTS *******************
-    // 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(
             RobotConstants.kRobotMass.in(Kilograms),
             RobotConstants.kRobotMOI,
             new ModuleConfig(
-                SwerveConstants.kWheelRadiusMeters,
+                DrivebaseConstants.kWheelRadiusMeters,
                 DrivebaseConstants.kMaxLinearSpeed,
                 RobotConstants.kWheelCOF,
                 DCMotor.getKrakenX60Foc(1).withReduction(SwerveConstants.kDriveGearRatio),
-                SwerveConstants.kDriveSlipCurrent,
+                DrivebaseConstants.kSlipCurrent,
                 1),
             Drive.getModuleTranslations());
 

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

@@ -140,7 +140,8 @@ public void robotPeriodic() {
   @Override
   public void disabledInit() {
     // Set the brakes to stop robot motion
-    m_robotContainer.setMotorBrake(true);
+    m_robotContainer.getDrivebase().setMotorBrake(true);
+    m_robotContainer.getDrivebase().resetHeadingController();
     m_disabledTimer.reset();
     m_disabledTimer.start();
   }
@@ -150,7 +151,7 @@ public void disabledInit() {
   public void disabledPeriodic() {
     // After WHEEL_LOCK_TIME has elapsed, release the drive brakes
     if (m_disabledTimer.hasElapsed(Constants.DrivebaseConstants.kWheelLockTime)) {
-      m_robotContainer.setMotorBrake(false);
+      m_robotContainer.getDrivebase().setMotorBrake(false);
       m_disabledTimer.stop();
     }
   }
@@ -161,7 +162,8 @@ public void autonomousInit() {
 
     // Just in case, cancel all running commands
     CommandScheduler.getInstance().cancelAll();
-    m_robotContainer.setMotorBrake(true);
+    m_robotContainer.getDrivebase().setMotorBrake(true);
+    m_robotContainer.getDrivebase().resetHeadingController();
 
     // TODO: Make sure Gyro inits here with whatever is in the path planning thingie
     switch (Constants.getAutoType()) {
@@ -202,7 +204,8 @@ public void teleopInit() {
     } else {
       CommandScheduler.getInstance().cancelAll();
     }
-    m_robotContainer.setMotorBrake(true);
+    m_robotContainer.getDrivebase().setMotorBrake(true);
+    m_robotContainer.getDrivebase().resetHeadingController();
 
     // In case this got set in sequential practice sessions or whatever
     FieldState.wonAuto = null;
@@ -246,6 +249,7 @@ public void teleopPeriodic() {
   public void testInit() {
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
+    m_robotContainer.getDrivebase().resetHeadingController();
   }
 
   /** This function is called periodically during test mode. */

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

@@ -64,7 +64,6 @@
 import frc.robot.util.OverrideSwitches;
 import frc.robot.util.RBSIEnum.AutoType;
 import frc.robot.util.RBSIEnum.Mode;
-import frc.robot.util.RBSIPowerMonitor;
 import java.util.Set;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.photonvision.PhotonCamera;
@@ -98,8 +97,8 @@ public class RobotContainer {
   @SuppressWarnings("unused")
   private final Accelerometer m_accel;
 
-  @SuppressWarnings("unused")
-  private final RBSIPowerMonitor m_power;
+  // @SuppressWarnings("unused")
+  // private final RBSIPowerMonitor m_power;
 
   @SuppressWarnings("unused")
   private final Vision m_vision;
@@ -217,7 +216,7 @@ public RobotContainer() {
     // In addition to the initial battery capacity from the Dashbaord, ``RBSIPowerMonitor`` takes
     // all the non-drivebase subsystems for which you wish to have power monitoring; DO NOT
     // include ``m_drivebase``, as that is automatically monitored.
-    m_power = new RBSIPowerMonitor(batteryCapacity, m_flywheel);
+    // m_power = new RBSIPowerMonitor(batteryCapacity, m_flywheel);
 
     // Set up the SmartDashboard Auto Chooser based on auto type
     switch (Constants.getAutoType()) {
@@ -442,11 +441,6 @@ public void getAutonomousCommandChoreo() {
     RobotModeTriggers.autonomous().whileTrue(autoChooserChoreo.selectedCommandScheduler());
   }
 
-  /** Set the motor neutral mode to BRAKE / COAST for T/F */
-  public void setMotorBrake(boolean brake) {
-    m_drivebase.setMotorBrake(brake);
-  }
-
   /** Updates the alerts. */
   public void updateAlerts() {
     // AprilTag layout alert
@@ -460,7 +454,7 @@ public void updateAlerts() {
     }
   }
 
-  /** Drivetrain getter method */
+  /** Drivetrain getter method for use with Robot.java */
   public Drive getDrivebase() {
     return m_drivebase;
   }

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

@@ -21,16 +21,13 @@
 
 import com.therekrab.autopilot.APTarget;
 import com.therekrab.autopilot.Autopilot.APResult;
-import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.units.measure.Distance;
 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.subsystems.drive.Drive;
 import org.littletonrobotics.junction.Logger;
 
@@ -178,16 +175,6 @@ public static Command runAutopilot(
    */
   private static Command autopilotToTarget(Drive drive, APTarget target) {
 
-    // Create PID controller
-    ProfiledPIDController angleController =
-        new ProfiledPIDController(
-            AutoConstants.kPPsteerPID.kP,
-            AutoConstants.kPPsteerPID.kI,
-            AutoConstants.kPPsteerPID.kD,
-            new TrapezoidProfile.Constraints(
-                DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel));
-    angleController.enableContinuousInput(-Math.PI, Math.PI);
-
     return Commands.run(
             () -> {
               ChassisSpeeds robotRelativeSpeeds = drive.getChassisSpeeds();
@@ -214,15 +201,19 @@ private static Command autopilotToTarget(Drive drive, APTarget target) {
                       output.vx(),
                       output.vy(),
                       RadiansPerSecond.of(
-                          angleController.calculate(
-                              drive.getHeading().getRadians(), output.targetAngle().getRadians())));
+                          drive
+                              .getAngleController()
+                              .calculate(
+                                  drive.getHeading().getRadians(),
+                                  output.targetAngle().getRadians())));
 
               drive.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, drive.getHeading()));
             },
             drive)
 
-        // Reset PID controller when command starts
-        .beforeStarting(() -> angleController.reset(drive.getHeading().getRadians()))
-        .until(() -> AutoConstants.kAutopilot.atTarget(drive.getPose(), target));
+        // Reset PID controller when command starts & ends; run until we're at target
+        .beforeStarting(() -> drive.resetHeadingController())
+        .until(() -> AutoConstants.kAutopilot.atTarget(drive.getPose(), target))
+        .finallyDo(() -> drive.resetHeadingController());
   }
 }

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

@@ -10,22 +10,18 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 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.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 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;
 import frc.robot.subsystems.drive.SwerveConstants;
@@ -117,16 +113,6 @@ public static Command fieldRelativeDriveAtAngle(
       DoubleSupplier ySupplier,
       Supplier<Rotation2d> rotationSupplier) {
 
-    // Create PID controller
-    ProfiledPIDController angleController =
-        new ProfiledPIDController(
-            AutoConstants.kPPsteerPID.kP,
-            AutoConstants.kPPsteerPID.kI,
-            AutoConstants.kPPsteerPID.kD,
-            new TrapezoidProfile.Constraints(
-                DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel));
-    angleController.enableContinuousInput(-Math.PI, Math.PI);
-
     // Construct command
     return Commands.run(
             () -> {
@@ -136,8 +122,10 @@ public static Command fieldRelativeDriveAtAngle(
 
               // Calculate angular speed
               double omega =
-                  angleController.calculate(
-                      drive.getHeading().getRadians(), rotationSupplier.get().getRadians());
+                  drive
+                      .getAngleController()
+                      .calculate(
+                          drive.getHeading().getRadians(), rotationSupplier.get().getRadians());
 
               // Convert to field relative speeds & send command
               ChassisSpeeds speeds =
@@ -157,8 +145,9 @@ public static Command fieldRelativeDriveAtAngle(
             },
             drive)
 
-        // Reset PID controller when command starts
-        .beforeStarting(() -> angleController.reset(drive.getHeading().getRadians()));
+        // Reset PID controller when command starts & ends;
+        .beforeStarting(() -> drive.resetHeadingController())
+        .finallyDo(() -> drive.resetHeadingController());
   }
 
   /** Utility functions needed by commands in this module ****************** */

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

@@ -15,7 +15,7 @@
 
 // import frc.robot.subsystems.CommandSwerveDrivetrain;
 
-// Generated by the Tuner X Swerve Project Generator
+// Generated by the 2026 Tuner X Swerve Project Generator
 // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
 public class TunerConstants {
   // Both sets of gains need to be tuned to your individual robot.