Misc drive fixes (#6)

* Implement Full Logging and Drivetrain Subsystem (#1)

* Da Code

* Add alerts on drive spark maxes

* Add Config Changes from Meeting

* Other lil bs

* Add automatic brake disable on robot disable

* add odometry

* add SIM module

* Add DriveCommands

* Update RobotContainer.java

* Formatting fixes

* Misc Fixes

* Add low voltage warning for DT

* Add velocity scalars for DT

* Update PID Coefficents and fix odometry issues

* Update URCL.json

---------

Fix CI

Co-Authored-By: Talon540-root <[email protected]>

* Increase voltage warning on battery and add CAN error alert

* clean

* Fix Modules Falsely Reporting as Disconnected

* Cleanup typing, make errors more obvious

* Cleanup dashboard setting stuff

* Update sim characterization constants to be more accurate, stops a massive overshooting for some reason

* Log drive motor temps

* Formatting fixes

* Inline tuning mode alert

---------

Co-authored-by: Talon540-root <[email protected]>

Author: srimanachanta

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

@@ -1,49 +1,47 @@
 package frc.robot;
 
-import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.RobotBase;
 
 public class Constants {
-  private static RobotType kRobotType = RobotType.ROBOT_2025_COMP;
+  private static RobotType robotType = RobotType.COMPBOT;
   // Allows tunable values to be changed when enabled. Also adds tunable selectors to AutoSelector
   public static final boolean TUNING_MODE = true;
   // Disable the AdvantageKit logger from running
   public static final boolean ENABLE_LOGGING = true;
 
   public static final double kLoopPeriodSecs = 0.02;
 
-  public static final double LOW_VOLTAGE_WARNING_THRESHOLD = 10.0;
-
-  public enum RobotMode {
-    REAL,
-    SIM,
-    REPLAY
+  public static RobotType getRobot() {
+    if (RobotBase.isReal() && robotType == RobotType.SIMBOT) {
+      new Alert(
+              "Invalid robot selected, using competition robot as default.", Alert.AlertType.kError)
+          .set(true);
+      robotType = RobotType.COMPBOT;
+    }
+    return robotType;
   }
 
-  public enum RobotType {
-    ROBOT_2025_COMP,
-    ROBOT_SIMBOT
+  public static Mode getMode() {
+    return switch (robotType) {
+      case COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY;
+      case SIMBOT -> Mode.SIM;
+    };
   }
 
-  public static RobotType getRobotType() {
-    if (RobotBase.isReal() && kRobotType == RobotType.ROBOT_SIMBOT) {
-      DriverStation.reportError(
-          "Robot is set to SIM but it isn't a SIM, setting it to Competition Robot as redundancy.",
-          false);
-      kRobotType = RobotType.ROBOT_2025_COMP;
-    }
+  public enum Mode {
+    /** Running on a real robot. */
+    REAL,
 
-    if (RobotBase.isSimulation() && kRobotType != RobotType.ROBOT_SIMBOT) {
-      DriverStation.reportError(
-          "Robot is set to REAL but it is a SIM, setting it to SIMBOT as redundancy.", false);
-      kRobotType = RobotType.ROBOT_SIMBOT;
-    }
+    /** Running a physics simulator. */
+    SIM,
 
-    return kRobotType;
+    /** Replaying from a log file. */
+    REPLAY
   }
 
-  public static RobotMode getRobotMode() {
-    if (getRobotType() == RobotType.ROBOT_SIMBOT) return RobotMode.SIM;
-    else return RobotBase.isReal() ? RobotMode.REAL : RobotMode.REPLAY;
+  public enum RobotType {
+    SIMBOT,
+    COMPBOT
   }
 }

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

@@ -15,6 +15,7 @@
 
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Threads;
@@ -36,19 +37,26 @@
  * project.
  */
 public class Robot extends LoggedRobot {
+  private static final double LOW_VOLTAGE_WARNING_THRESHOLD = 11.75;
+
   private Command autonomousCommand;
   private final RobotContainer robotContainer;
 
+  // System Alerts
+  private final Alert canErrorAlert =
+      new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError);
+  private final Debouncer canErrorDebouncer = new Debouncer(0.5);
+
   private final Alert lowBatteryVoltageAlert =
       new Alert("Battery voltage is too low, change the battery", Alert.AlertType.kWarning);
-  private final Debouncer batteryVoltageDebouncer = new Debouncer(0.5);
+  private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5);
 
   public Robot() {
     super(Constants.kLoopPeriodSecs);
 
     LoggerUtil.initializeLoggerMetadata();
 
-    switch (Constants.getRobotMode()) {
+    switch (Constants.getMode()) {
       case REAL -> {
         // Running on a real robot, log to a USB stick
         var loggerPath = LoggerUtil.getLogPath();
@@ -92,10 +100,16 @@ public void robotPeriodic() {
     // Run command scheduler
     CommandScheduler.getInstance().run();
 
+    // Check CAN status
+    var canStatus = RobotController.getCANStatus();
+    canErrorAlert.set(
+        canErrorDebouncer.calculate(
+            canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0));
+
     // Update Battery Voltage Alert
     lowBatteryVoltageAlert.set(
         batteryVoltageDebouncer.calculate(
-            RobotController.getBatteryVoltage() <= Constants.LOW_VOLTAGE_WARNING_THRESHOLD));
+            RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD));
 
     // Return to normal thread priority
     Threads.setCurrentThreadPriority(false, 10);

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

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -24,7 +25,7 @@ public class RobotContainer {
   private final LoggedDashboardChooser<Command> autoChooser;
 
   public RobotContainer() {
-    switch (Constants.getRobotMode()) {
+    switch (Constants.getMode()) {
       case REAL -> {
         driveBase =
             new DriveBase(
@@ -58,6 +59,8 @@ public RobotContainer() {
     autoChooser = new LoggedDashboardChooser<>("Auto Choices");
 
     if (Constants.TUNING_MODE) {
+      new Alert("Robot in Tuning Mode", Alert.AlertType.kInfo).set(true);
+
       // Set up Characterization routines
       autoChooser.addOption(
           "Drive Wheel Radius Characterization",

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

@@ -15,22 +15,22 @@
 import frc.robot.subsystems.drive.DriveBase;
 import frc.robot.subsystems.drive.DriveConstants;
 import frc.robot.util.AllianceFlipUtil;
-import frc.robot.util.LoggedTunableNumber;
 import java.text.DecimalFormat;
 import java.text.NumberFormat;
 import java.util.LinkedList;
 import java.util.List;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
 
 public class DriveCommands {
   // Drive
   private static final double DEADBAND = 0.1;
 
-  private static final LoggedTunableNumber LINEAR_VELOCITY_SCALAR =
-      new LoggedTunableNumber("TeleopDrive/LinearVelocityScalar", 1.0, true);
-  private static final LoggedTunableNumber ANGULAR_VELOCITY_SCALAR =
-      new LoggedTunableNumber("TeleopDrive/AngularVelocityScalar", 1.0, true);
+  private static final LoggedNetworkNumber LINEAR_VELOCITY_SCALAR =
+      new LoggedNetworkNumber("TeleopDrive/LinearVelocityScalar", 1.0);
+  private static final LoggedNetworkNumber ANGULAR_VELOCITY_SCALAR =
+      new LoggedNetworkNumber("TeleopDrive/AngularVelocityScalar", 0.7);
 
   private static final double ANGLE_KP = 5.0;
   private static final double ANGLE_KD = 0.4;
@@ -66,6 +66,7 @@ public static Command joystickDrive(
           // Generate robot relative speeds
           double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get();
           double angularVelocityScalar = ANGULAR_VELOCITY_SCALAR.get();
+
           var speeds =
               new ChassisSpeeds(
                   x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar,

src/main/java/frc/robot/subsystems/drive/DriveBase.java

@@ -154,8 +154,7 @@ public void periodic() {
     }
 
     // Update gyro alert
-    gyroDisconnectedAlert.set(
-        !m_gyroInputs.connected && Constants.getRobotMode() != Constants.RobotMode.SIM);
+    gyroDisconnectedAlert.set(!m_gyroInputs.connected && Constants.getMode() != Constants.Mode.SIM);
   }
 
   /** Set brake mode to {@code enabled} doesn't change brake mode if already set. */

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -9,7 +9,7 @@
 
 public class DriveConstants {
   public static final double odometryFrequencyHz =
-      Constants.getRobotMode() == Constants.RobotMode.SIM ? 50 : 250;
+      Constants.getMode() == Constants.Mode.SIM ? 50 : 250;
 
   public static final double trackWidthX = Units.inchesToMeters(20.75);
   public static final double trackWidthY = Units.inchesToMeters(20.75);

src/main/java/frc/robot/subsystems/drive/Module.java

@@ -24,8 +24,8 @@ public class Module {
   private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD");
 
   static {
-    switch (Constants.getRobotType()) {
-      case ROBOT_2025_COMP -> {
+    switch (Constants.getRobot()) {
+      case COMPBOT -> {
         drivekS.initDefault(0.19700);
         drivekV.initDefault(0.12941);
         drivekP.initDefault(0.005);
@@ -34,8 +34,8 @@ public class Module {
         turnkD.initDefault(0.05);
       }
       default -> {
-        drivekS.initDefault(0.11400);
-        drivekV.initDefault(0.84144);
+        drivekS.initDefault(0.113190);
+        drivekV.initDefault(0.841640);
         drivekP.initDefault(0.1);
         drivekD.initDefault(0.0);
         turnkP.initDefault(10.0);

src/main/java/frc/robot/subsystems/drive/ModuleIO.java

@@ -11,13 +11,15 @@ public static class ModuleIOInputs {
     public double driveVelocityRadPerSec = 0.0;
     public double driveAppliedVolts = 0.0;
     public double driveCurrentAmps = 0.0;
+    public double driveTemperatureCelsius = 0.0;
 
     public boolean turnConnected = false;
     public Rotation2d turnAbsolutePosition = new Rotation2d();
     public Rotation2d turnPosition = new Rotation2d();
     public double turnVelocityRadPerSec = 0.0;
     public double turnAppliedVolts = 0.0;
     public double turnCurrentAmps = 0.0;
+    public double turnTemperatureCelsius = 0.0;
 
     public double[] odometryDrivePositionsRad = new double[] {};
     public Rotation2d[] odometryTurnPositions = new Rotation2d[] {};

src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java

@@ -46,13 +46,13 @@ public class ModuleIOSpark implements ModuleIO {
   private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
 
   public ModuleIOSpark(int index) {
-    switch (Constants.getRobotType()) {
-      case ROBOT_2025_COMP -> {
+    switch (Constants.getRobot()) {
+      case COMPBOT -> {
         config = moduleConfigs[index];
       }
       default ->
           throw new IllegalStateException(
-              "Unexpected RobotType for Spark Module: " + Constants.getRobotType());
+              "Unexpected RobotType for Spark Module: " + Constants.getRobot());
     }
 
     // Initialize Hardware Devices
@@ -134,15 +134,17 @@ public void updateInputs(ModuleIOInputs inputs) {
     inputs.driveVelocityRadPerSec = driveEncoder.getVelocity();
     inputs.driveAppliedVolts = driveSpark.getAppliedOutput() * driveSpark.getBusVoltage();
     inputs.driveCurrentAmps = driveSpark.getOutputCurrent();
+    inputs.driveTemperatureCelsius = driveSpark.getMotorTemperature();
 
     inputs.turnAbsolutePosition = getOffsetAbsoluteAngle();
     inputs.turnPosition = Rotation2d.fromRadians(turnEncoder.getPosition());
     inputs.turnVelocityRadPerSec = turnEncoder.getVelocity();
     inputs.turnAppliedVolts = turnSpark.getAppliedOutput() * turnSpark.getBusVoltage();
     inputs.turnCurrentAmps = turnSpark.getOutputCurrent();
+    inputs.turnTemperatureCelsius = turnSpark.getMotorTemperature();
 
-    inputs.driveConnected = driveConnectedDebounce.calculate(driveSpark.hasActiveFault());
-    inputs.turnConnected = turnConnectedDebounce.calculate(turnSpark.hasActiveFault());
+    inputs.driveConnected = driveConnectedDebounce.calculate(!driveSpark.hasActiveFault());
+    inputs.turnConnected = turnConnectedDebounce.calculate(!turnSpark.hasActiveFault());
 
     inputs.odometryDrivePositionsRad =
         drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray();

src/main/java/frc/robot/util/EqualsUtil.java

@@ -18,5 +18,11 @@ public static boolean epsilonEquals(Twist2d twist, Twist2d other) {
           && EqualsUtil.epsilonEquals(twist.dy, other.dy)
           && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta);
     }
+
+    public static boolean equalsZero(Twist2d twist) {
+      return EqualsUtil.epsilonEquals(twist.dx, 0.0)
+          && EqualsUtil.epsilonEquals(twist.dy, 0.0)
+          && EqualsUtil.epsilonEquals(twist.dtheta, 0.0);
+    }
   }
 }

src/main/java/frc/robot/util/LoggedTunableNumber.java

@@ -20,38 +20,13 @@ public class LoggedTunableNumber {
 
   private LoggedNetworkNumber dashboardNumber;
 
-  private final boolean ntPubEnabled;
-
-  /**
-   * Create a new LoggedTunableNumber
-   *
-   * @param dashboardKey Key on dashboard
-   * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode
-   */
-  public LoggedTunableNumber(String dashboardKey, boolean alwaysEnabled) {
-    this.key = tableKey + "/" + dashboardKey;
-    this.ntPubEnabled = alwaysEnabled;
-  }
-
   /**
    * Create a new LoggedTunableNumber
    *
    * @param dashboardKey Key on dashboard
    */
   public LoggedTunableNumber(String dashboardKey) {
-    this(dashboardKey, Constants.TUNING_MODE);
-  }
-
-  /**
-   * Create a new LoggedTunableNumber with the default value
-   *
-   * @param dashboardKey Key on dashboard
-   * @param defaultValue Default value
-   * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode
-   */
-  public LoggedTunableNumber(String dashboardKey, double defaultValue, boolean alwaysEnabled) {
-    this(dashboardKey, alwaysEnabled);
-    initDefault(defaultValue);
+    this.key = tableKey + "/" + dashboardKey;
   }
 
   /**
@@ -80,7 +55,7 @@ public void initDefault(double defaultValue) {
     }
 
     this.defaultValue = defaultValue;
-    if (ntPubEnabled) {
+    if (Constants.TUNING_MODE) {
       dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
     }
   }
@@ -99,7 +74,7 @@ public double get() {
               key));
     }
 
-    return ntPubEnabled ? dashboardNumber.get() : defaultValue;
+    return Constants.TUNING_MODE ? dashboardNumber.get() : defaultValue;
   }
 
   /**

src/main/java/frc/robot/util/LoggerUtil.java

@@ -11,7 +11,7 @@ public class LoggerUtil {
   /** Initialize the Logger with the auto-generated data from the build. */
   public static void initializeLoggerMetadata() {
     // Record metadata from generated state file.
-    Logger.recordMetadata("ROBOT_NAME", Constants.getRobotType().toString());
+    Logger.recordMetadata("ROBOT_NAME", Constants.getRobot().toString());
     Logger.recordMetadata("RUNTIME_ENVIRONMENT", RobotBase.getRuntimeType().toString());
     Logger.recordMetadata("TUNING_MODE", Boolean.toString(Constants.TUNING_MODE));
     Logger.recordMetadata("PROJECT_NAME", BuildConstants.MAVEN_NAME);