Logging and Tuning Improvements

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

@@ -19,6 +19,7 @@
 
 import static edu.wpi.first.units.Units.*;
 
+import com.ctre.phoenix6.CANBus;
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
@@ -45,6 +46,9 @@
 import frc.robot.util.RBSIEnum.SwerveType;
 import frc.robot.util.RBSIEnum.VisionType;
 import frc.robot.util.RobotDeviceId;
+import java.util.Collections;
+import java.util.HashMap;
+import java.util.Map;
 import org.photonvision.simulation.SimCameraProperties;
 import swervelib.math.Matter;
 
@@ -161,13 +165,56 @@ public static final class PowerConstants {
     public static final double kVoltageCritical = 6.5;
   }
 
+  /************************************************************************* */
+  /** List of Robot CAN Busses ********************************************* */
+  public static final class CANBuses {
+
+    // ---- Bus names (single source of truth) ----
+    public static final String DRIVE = "DriveTrain";
+    public static final String RIO = "";
+    // In 2027 and later, you'll be able to have even more CAN Busses!
+
+    // ---- Singleton instances (exactly one per bus) ----
+    public static final CANBus DRIVE_BUS = new CANBus(DRIVE);
+    public static final CANBus RIO_BUS = new CANBus(RIO);
+
+    // ---- Lookup table: name -> CANBus singleton ----
+    private static final Map<String, CANBus> BY_NAME;
+
+    static {
+      Map<String, CANBus> m = new HashMap<>();
+      m.put(DRIVE, DRIVE_BUS);
+      m.put(RIO, RIO_BUS);
+      BY_NAME = Collections.unmodifiableMap(m);
+    }
+
+    /**
+     * Get the singleton CANBus for a given bus name.
+     *
+     * <p>Usage: CANBus bus = Constants.CANBuses.get(Constants.CANBuses.DRIVE);
+     */
+    public static CANBus get(String name) {
+      CANBus bus = BY_NAME.get(name);
+      if (bus == null) {
+        throw new IllegalArgumentException(
+            "Unknown CAN bus name '" + name + "'. Known buses: " + BY_NAME.keySet());
+      }
+      return bus;
+    }
+
+    /** Expose known bus names for debugging. */
+    public static Map<String, CANBus> all() {
+      return BY_NAME;
+    }
+  }
+
   /************************************************************************* */
   /** List of Robot Device CAN and Power Distribution Circuit IDs ********** */
   public static class RobotDevices {
 
     /* DRIVETRAIN CAN DEVICE IDS */
     // Input the correct Power Distribution Module port for each motor!!!!
-    // NOTE: The CAN ID and bus are set in the Swerve Generator (Phoenix Tuner or YAGSL)
+    // NOTE: The CAN ID's are set in the Swerve Generator (Phoenix Tuner or YAGSL)
 
     // Front Left
     public static final RobotDeviceId FL_DRIVE =
@@ -204,8 +251,8 @@ public static class RobotDevices {
     /* SUBSYSTEM CAN DEVICE IDS */
     // This is where mechanism subsystem devices are defined (Including ID, bus, and power port)
     // Example:
-    public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, "", 8);
-    public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, "", 9);
+    public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, CANBuses.RIO, 8);
+    public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, CANBuses.RIO, 9);
 
     /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */
     // This is where digital I/O feedback devices are defined
@@ -262,19 +309,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 +381,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 +409,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

@@ -112,35 +112,60 @@ public 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();
+
+    // Switch thread to high priority to improve loop timing
+    if (isReal()) {
+      Threads.setCurrentThreadPriority(true, 99);
+    }
   }
 
-  /** This function is called periodically during all modes. */
+  // /** This function is called periodically during all modes. */
+  // @Override
+  // public void robotPeriodic() {
+
+  //   // Run all virtual subsystems each time through the loop
+  //   VirtualSubsystem.periodicAll();
+
+  //   // Runs the Scheduler. This is responsible for polling buttons, adding
+  //   // newly-scheduled commands, running already-scheduled commands, removing
+  //   // finished or interrupted commands, and running subsystem periodic() methods.
+  //   // This must be called from the robot's periodic block in order for anything in
+  //   // the Command-based framework to work.
+  //   CommandScheduler.getInstance().run();
+  // }
+
+  /** TESTING VERSION OF ROBOTPERIODIC FOR OVERRUN SOURCES */
   @Override
   public void robotPeriodic() {
-    // Switch thread to high priority to improve loop timing
+    final long t0 = System.nanoTime();
+
     if (isReal()) {
       Threads.setCurrentThreadPriority(true, 99);
     }
+    final long t1 = System.nanoTime();
 
-    // Run all virtual subsystems each time through the loop
     VirtualSubsystem.periodicAll();
+    final long t2 = System.nanoTime();
 
-    // Runs the Scheduler. This is responsible for polling buttons, adding
-    // newly-scheduled commands, running already-scheduled commands, removing
-    // finished or interrupted commands, and running subsystem periodic() methods.
-    // This must be called from the robot's periodic block in order for anything in
-    // the Command-based framework to work.
     CommandScheduler.getInstance().run();
+    final long t3 = System.nanoTime();
 
-    // Return to normal thread priority
     Threads.setCurrentThreadPriority(false, 10);
+    final long t4 = System.nanoTime();
+
+    Logger.recordOutput("Loop/RobotPeriodic_ms", (t4 - t0) / 1e6);
+    Logger.recordOutput("Loop/ThreadBoost_ms", (t1 - t0) / 1e6);
+    Logger.recordOutput("Loop/Virtual_ms", (t2 - t1) / 1e6);
+    Logger.recordOutput("Loop/Scheduler_ms", (t3 - t2) / 1e6);
+    Logger.recordOutput("Loop/ThreadRestore_ms", (t4 - t3) / 1e6);
   }
 
   /** This function is called once when the robot is disabled. */
   @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 +175,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 +186,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 +228,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 +273,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. */