From 2c8d4ef79e2fc9322430b2a2e7684d78de490610 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sat, 26 Sep 2026 13:29:08 -0400 Subject: [PATCH] Raise WPILib alerts for Phoenix device and CAN bus health Phoenix 6 sends its automatic alerts straight to MrcLib, so they never reach WPILib's alert registry and AdvantageKit doesn't log them (#47). This covers the conditions that matter to this robot with WPILib Alerts, computed from logged inputs so replay reproduces them. Config failure: tryUntilOk returns its last StatusCode, and each module logs its drive setup, turn config, and CANcoder read and write results, with a HIGH alert per failure. A failed CANcoder config read skips the write and never seeds the turn-zero Preference, since the config then holds defaults. The read result stays latched. The setTurnZero write makes one attempt, because it runs on the main loop. Firmware: Phoenix blocks a motor's output only on a firmware/API compliancy mismatch, and setControl returns FirmwareTooOld or ApiTooOld when it does (ParentDevice.setControlPrivate). Each module keeps the setControl result and raises a HIGH alert when output is blocked. Firmware versions are logged for reference. A CANcoder disconnected alert joins the drive and turn ones. CAN bus (#50): LoggedCANBus reads CANBus.getStatus() on a background thread every 400 ms in REAL mode, since the call can block for up to 1 ms. It logs BusErrorCount, ArbitrationLostCount, RestartCount, State, Status and a sample count. CANBusHealth raises a HIGH alert for ErrorPassive, BusOff, Stopped, a failed status read, a rising bus-off or restart count, or a stalled reader, and a MEDIUM alert for ErrorWarning, each held 0.5 s. REC and TEC stay log-only. Hoot logging: FeatureFlags.HOOT_LOGGING_ENABLED, off by default, disables Phoenix's automatic .hoot logging in every mode. Verified in sim with ModuleIOSimTalonFX and injected failures (temporary, not committed): hoot file 446 KB with the flag on and none with it off; driveInitStatus=ConfigFailed and the CANcoder read-failure alert shown and held; the turn-zero Preference re-seeded after a good read and not after a failed one; firmware decode 0x1A460000 -> 26.70.0.0 matching getVersionMajor/Minor/Bugfix/Build. The default sim shows no alerts. Closes #52 Co-Authored-By: Claude Opus 5.5 --- .../java/frc/lib/hardware/CANBusHealth.java | 125 +++++++++++++++++ .../java/frc/lib/hardware/LoggedCANBus.java | 114 +++++++++++++++- .../frc/lib/hardware/PhoenixFirmware.java | 50 +++++++ src/main/java/frc/robot/Constants.java | 3 + src/main/java/frc/robot/Robot.java | 39 ++++-- .../frc/robot/subsystems/drive/Drive.java | 4 +- .../frc/robot/subsystems/drive/Module.java | 76 ++++++++++- .../frc/robot/subsystems/drive/ModuleIO.java | 15 ++ .../subsystems/drive/ModuleIOTalonFXBase.java | 127 ++++++++++++----- .../frc/robot/util/odometry/PhoenixUtil.java | 22 ++- .../frc/lib/hardware/CANBusHealthTest.java | 128 ++++++++++++++++++ .../frc/lib/hardware/PhoenixFirmwareTest.java | 48 +++++++ .../robot/util/odometry/PhoenixUtilTest.java | 51 +++++++ 13 files changed, 741 insertions(+), 61 deletions(-) create mode 100644 src/main/java/frc/lib/hardware/CANBusHealth.java create mode 100644 src/main/java/frc/lib/hardware/PhoenixFirmware.java create mode 100644 src/test/java/frc/lib/hardware/CANBusHealthTest.java create mode 100644 src/test/java/frc/lib/hardware/PhoenixFirmwareTest.java create mode 100644 src/test/java/frc/robot/util/odometry/PhoenixUtilTest.java diff --git a/src/main/java/frc/lib/hardware/CANBusHealth.java b/src/main/java/frc/lib/hardware/CANBusHealth.java new file mode 100644 index 0000000..de428d7 --- /dev/null +++ b/src/main/java/frc/lib/hardware/CANBusHealth.java @@ -0,0 +1,125 @@ +// Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.lib.hardware; + +/** + * Decides whether a CAN bus needs an alert, from one cycle of logged bus status at a time. + * + *

The states follow the CAN standard's fault confinement: a controller enters ErrorWarning when + * an error counter reaches 96, ErrorPassive at 128, and BusOff when the transmit counter passes + * 255. A few error frames on a healthy bus raise the counters without reaching those states, so the + * raw counters are logged but do not trigger alerts. + * + *

This class uses only strings, numbers and the caller's timestamps, so it gives the same result + * in log replay and can be unit tested without Phoenix or the HAL. + */ +public class CANBusHealth { + /** How urgent a bus problem is. */ + public enum Severity { + NONE, + MEDIUM, + HIGH + } + + /** How long an alert stays active after the condition was last seen, in seconds. */ + public static final double HOLD_SECONDS = 0.5; + + /** How long the sample count may stay unchanged before the reader counts as stalled. */ + public static final double STALE_SECONDS = 1.5; + + private boolean hasPrevious = false; + private long previousSampleCount; + private long previousBusOffCount; + private long previousRestartCount; + private double lastSampleChangeTime; + private double lastHighTime = Double.NEGATIVE_INFINITY; + private double lastMediumTime = Double.NEGATIVE_INFINITY; + + /** + * Returns the severity of one bus status sample. + * + * @param status the Phoenix StatusCode name of the status read ("OK" when the read worked) + * @param state the Phoenix CANState name + * @param busOffRose whether the bus-off count rose since the previous sample + * @param restartRose whether the controller restart count rose since the previous sample + * @param stale whether the reader has stopped producing samples + */ + public static Severity severity( + String status, String state, boolean busOffRose, boolean restartRose, boolean stale) { + if (!"OK".equals(status) + || "ErrorPassive".equals(state) + || "BusOff".equals(state) + || "Stopped".equals(state) + || busOffRose + || restartRose + || stale) { + return Severity.HIGH; + } + if ("ErrorWarning".equals(state)) return Severity.MEDIUM; + return Severity.NONE; + } + + /** + * Takes one cycle of logged bus status. + * + *

A sample count of 0 means no status has been read, as in simulation or when replaying a log + * recorded without a reader. Such cycles never count as stale. Counter increases are measured + * from the first real sample, so counts left from before a code restart do not trigger an alert. + * + * @param now the current timestamp in seconds + * @param sampleCount how many status reads the reader has completed + * @param status the Phoenix StatusCode name of the status read + * @param state the Phoenix CANState name + * @param busOffCount the cumulative bus-off count + * @param restartCount the cumulative controller restart count + */ + public void update( + double now, + long sampleCount, + String status, + String state, + long busOffCount, + long restartCount) { + boolean busOffRose = false; + boolean restartRose = false; + boolean stale = false; + if (sampleCount > 0) { + if (!hasPrevious) { + hasPrevious = true; + previousSampleCount = sampleCount; + lastSampleChangeTime = now; + } else { + busOffRose = busOffCount > previousBusOffCount; + restartRose = restartCount > previousRestartCount; + if (sampleCount != previousSampleCount) { + previousSampleCount = sampleCount; + lastSampleChangeTime = now; + } + stale = now - lastSampleChangeTime > STALE_SECONDS; + } + previousBusOffCount = busOffCount; + previousRestartCount = restartCount; + } + + switch (severity(status, state, busOffRose, restartRose, stale)) { + case HIGH -> lastHighTime = now; + case MEDIUM -> lastMediumTime = now; + case NONE -> {} + } + } + + /** Returns true while a high-severity problem was seen within the last {@link #HOLD_SECONDS}. */ + public boolean isHighActive(double now) { + return now - lastHighTime < HOLD_SECONDS; + } + + /** Returns true while a warning was seen within the last {@link #HOLD_SECONDS}. */ + public boolean isMediumActive(double now) { + return now - lastMediumTime < HOLD_SECONDS; + } +} diff --git a/src/main/java/frc/lib/hardware/LoggedCANBus.java b/src/main/java/frc/lib/hardware/LoggedCANBus.java index d3ec99f..9fe0828 100644 --- a/src/main/java/frc/lib/hardware/LoggedCANBus.java +++ b/src/main/java/frc/lib/hardware/LoggedCANBus.java @@ -8,9 +8,21 @@ package frc.lib.hardware; import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.CANBus.CANBusStatus; import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; +import org.wpilib.driverstation.DriverStationErrors; +import org.wpilib.system.Timer; +import org.wpilib.util.Alert; +/** + * Logs a CAN bus's controller status from Phoenix and raises alerts when the bus is in trouble. + * + *

{@link CANBus#getStatus()} can block for up to 1 ms (its Javadoc), so a background thread + * reads it and the main loop only copies the latest sample. Without {@link #start()}, as in + * simulation, the inputs keep their defaults. Alerts come only from the logged inputs, so replay + * reproduces them. + */ public class LoggedCANBus { @AutoLog public static class CANBusStatusInputs { @@ -19,11 +31,29 @@ public static class CANBusStatusInputs { public long txFullCount = 0; public long receiveErrorCount = 0; public long transmitErrorCount = 0; + public long busErrorCount = 0; + public long arbitrationLostCount = 0; + public long restartCount = 0; + public String state = "ErrorActive"; + public String status = "OK"; + public long sampleCount = 0; } + /** How often the background thread reads the bus status, in milliseconds. */ + private static final long SAMPLE_PERIOD_MS = 400; + + /** One status read, paired with its sequence number so the two are published together. */ + private record Sample(CANBusStatus status, long sequence) {} + private final CANBus bus; + private final String name; private final String key; private final CANBusStatusInputsAutoLogged inputs = new CANBusStatusInputsAutoLogged(); + private final CANBusHealth health = new CANBusHealth(); + private final Alert errorAlert; + private final Alert warningAlert; + private volatile Sample latest = null; + private Thread reader = null; /** * Creates a logged CAN bus status reporter. @@ -33,16 +63,88 @@ public static class CANBusStatusInputs { */ public LoggedCANBus(String name, CANBus bus) { this.bus = bus; + this.name = name; this.key = "CANBus/" + name; + errorAlert = new Alert("CANBus/" + name + "/errors", "", Alert.Level.HIGH); + warningAlert = new Alert("CANBus/" + name + "/warning", "", Alert.Level.MEDIUM); + } + + /** Reads the status once, then starts the background thread that keeps reading it. */ + public synchronized void start() { + if (reader != null) return; + latest = new Sample(bus.getStatus(), 1); + reader = new Thread(this::readLoop, "CANBusReader-" + name); + reader.setDaemon(true); + reader.start(); + } + + private void readLoop() { + long sequence = latest.sequence(); + boolean warned = false; + while (!Thread.currentThread().isInterrupted()) { + try { + Thread.sleep(SAMPLE_PERIOD_MS); + } catch (InterruptedException e) { + return; + } + try { + sequence++; + latest = new Sample(bus.getStatus(), sequence); + } catch (RuntimeException e) { + // Keep reading. A stalled sample count raises the stale alert. + if (!warned) { + DriverStationErrors.reportWarning( + "CAN status read failed on " + name + ": " + e.getMessage(), false); + warned = true; + } + } + } } public void log() { - var status = bus.getStatus(); - inputs.busUtilization = status.BusUtilization; - inputs.busOffCount = status.BusOffCount; - inputs.txFullCount = status.TxFullCount; - inputs.receiveErrorCount = status.REC; - inputs.transmitErrorCount = status.TEC; + Sample sample = latest; + if (sample != null) { + CANBusStatus status = sample.status(); + inputs.busUtilization = status.BusUtilization; + inputs.busOffCount = status.BusOffCount; + inputs.txFullCount = status.TxFullCount; + inputs.receiveErrorCount = status.REC; + inputs.transmitErrorCount = status.TEC; + inputs.busErrorCount = status.BusErrorCount; + inputs.arbitrationLostCount = status.ArbitrationLostCount; + inputs.restartCount = status.RestartCount; + inputs.state = String.valueOf(status.State); + inputs.status = status.Status.getName(); + inputs.sampleCount = sample.sequence(); + } Logger.processInputs(key, inputs); + + double now = Timer.getTimestamp(); + health.update( + now, + inputs.sampleCount, + inputs.status, + inputs.state, + inputs.busOffCount, + inputs.restartCount); + setAlert( + errorAlert, + health.isHighActive(now), + "CAN errors on " + + name + + " (" + + inputs.state + + ", " + + inputs.status + + "); robot may not be controllable."); + setAlert( + warningAlert, + health.isMediumActive(now), + "CAN error warning on " + name + " (" + inputs.state + ")."); + } + + private static void setAlert(Alert alert, boolean active, String text) { + if (active && !text.equals(alert.getText())) alert.setText(text); + alert.set(active); } } diff --git a/src/main/java/frc/lib/hardware/PhoenixFirmware.java b/src/main/java/frc/lib/hardware/PhoenixFirmware.java new file mode 100644 index 0000000..022177c --- /dev/null +++ b/src/main/java/frc/lib/hardware/PhoenixFirmware.java @@ -0,0 +1,50 @@ +// Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.lib.hardware; + +/** + * Pure helpers for Phoenix 6 firmware status. They take plain strings and ints, not Phoenix types, + * so they can be unit tested without loading Phoenix. + */ +public final class PhoenixFirmware { + private PhoenixFirmware() {} + + /** + * Returns true when a {@code setControl} status means Phoenix is blocking the motor's output. + * + *

Phoenix compares the device's firmware compliancy with the API's. On a mismatch it sends a + * neutral output instead of the request and returns {@code FirmwareTooOld} or {@code ApiTooOld} + * from {@code setControl} ({@code ParentDevice.setControlPrivate}, Phoenix 26.70.0-alpha-2). + * + * @param controlStatus the {@code StatusCode} name returned by {@code setControl} + */ + public static boolean isBlocked(String controlStatus) { + return "FirmwareTooOld".equals(controlStatus) || "ApiTooOld".equals(controlStatus); + } + + /** + * Formats a device's four-byte {@code getVersion()} value as "major.minor.bugfix.build". + * + *

The byte order, most significant byte first, is an assumption: Phoenix documents only a + * "four byte value". Compare the result with {@code getVersionMajor()} and the other per-byte + * signals before relying on it. + * + * @param version the raw version value + * @return the formatted version, or "" for 0, which means the version is unknown + */ + public static String format(int version) { + if (version == 0) return ""; + return ((version >>> 24) & 0xFF) + + "." + + ((version >>> 16) & 0xFF) + + "." + + ((version >>> 8) & 0xFF) + + "." + + (version & 0xFF); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5cb9e24..8eb4133 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,6 +33,9 @@ public static final class FeatureFlags { /** Enable to add the module forces from Choreo trajectories as drive feedforward. */ public static final boolean TRAJECTORY_FORCE_FF = false; + + /** Enable to let Phoenix write .hoot signal logs. Off by default. */ + public static final boolean HOOT_LOGGING_ENABLED = false; } public final class RobotConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7565ab9..794a7c7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -82,17 +82,18 @@ * project. */ public class Robot extends LoggedRobot { - // SESSION_DIR and SignalLogger.setPath() must be initialized before any CTRE device is - // constructed. A static initializer guarantees this runs before the constructor or any - // instance field initializer that could trigger CANHD class loading. + // Phoenix starts writing .hoot signal logs on its own, on SystemCore and in simulation. It starts + // once the robot is enabled at least 1 s after startup, or at least 5 s after startup with the + // DS connected (in simulation, at 5 s without a DS). The logging choice must be made before + // then. This static initializer runs before the constructor and every instance field. private static final String SESSION_DIR; static { - if (Constants.currentMode == RobotMode.REAL) { - SESSION_DIR = createSessionDir(); - SignalLogger.setPath(SESSION_DIR); + SESSION_DIR = Constants.currentMode == RobotMode.REAL ? createSessionDir() : null; + if (FeatureFlags.HOOT_LOGGING_ENABLED) { + if (SESSION_DIR != null) SignalLogger.setPath(SESSION_DIR); } else { - SESSION_DIR = null; + SignalLogger.enableAutoLogging(false); } } @@ -112,6 +113,9 @@ public class Robot extends LoggedRobot { private RobotStats robotStats; + /** Time to construct the drive with real hardware, in milliseconds. Measured in REAL mode. */ + private double driveConstructMs = 0.0; + // Subsystems private Drive drive; private Vision vision; @@ -142,12 +146,14 @@ public Robot() { // Set up data receivers & replay source switch (Constants.currentMode) { case REAL: // Running on a real robot - // SESSION_DIR and SignalLogger.setPath() were already set in the static initializer. - // SignalLogger will create a nested timestamp subdir inside SESSION_DIR for hoot files. + // The static initializer created SESSION_DIR. With hoot logging enabled, Phoenix writes + // its .hoot files to a timestamped subdirectory of it. Logger.addDataReceiver(new WPILOGWriter(SESSION_DIR)); Logger.addDataReceiver(new NT4Publisher()); - // Instantiate hardware IO implementations + // Instantiate hardware IO implementations. Device setup retries on failure, so a missing + // device slows this down. Timed to measure boot cost. + long driveConstructStart = System.nanoTime(); drive = new Drive( new GyroIOBoron(), @@ -155,6 +161,7 @@ public Robot() { new ModuleIOTalonFX(DriveConstants.FRONT_RIGHT), new ModuleIOTalonFX(DriveConstants.BACK_LEFT), new ModuleIOTalonFX(DriveConstants.BACK_RIGHT)); + driveConstructMs = (System.nanoTime() - driveConstructStart) / 1e6; if (FeatureFlags.VISION_ENABLED) { vision = new Vision( @@ -227,9 +234,16 @@ public Robot() { SparkOdometryThread.getInstance().start(); if (FeatureFlags.VISION_ENABLED) VisionThread.getInstance().start(); CanandgyroThread.getInstance().start(); + if (Constants.currentMode == RobotMode.REAL) { + sc0CANBus.start(); + sc1CANBus.start(); + } // Start AdvantageKit logger Logger.start(); + if (Constants.currentMode == RobotMode.REAL) { + Logger.recordOutput("Drive/ConstructMs", driveConstructMs); + } robotStats = new RobotStats(drive::getTotalDistanceTraveledMeters); @@ -580,9 +594,8 @@ private void logScheduler() { } /** - * Creates and returns a timestamped session directory under /U/logs/. Must be called before any - * CTRE devices are constructed so that SignalLogger.setPath() takes effect before auto-logging - * begins. + * Creates and returns a numbered session directory under /U/logs/. AdvantageKit writes its log + * there, and Phoenix writes .hoot files there when hoot logging is enabled. */ private static String createSessionDir() { java.io.File logsDir = new java.io.File("/U/logs"); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 38d0788..61e95cc 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -159,7 +159,9 @@ public void periodic() { long t4 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; ODOMETRY_LOCK.unlock(); - // Stop moving when disabled + // Stop the modules on every loop while disabled. Phoenix keeps re-sending a motor's last + // control request, so this keeps an old setpoint from resuming at enable. It also keeps each + // motor's setControl status current, which the firmware-blocked alerts in Module read. if (RobotState.isDisabled()) { for (var module : modules) { module.stop(); diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 2cc9f4b..8500e35 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -12,6 +12,7 @@ import static frc.robot.subsystems.drive.DriveConstants.*; +import frc.lib.hardware.PhoenixFirmware; import frc.robot.Constants.FeatureFlags; import org.littletonrobotics.junction.Logger; import org.wpilib.math.geometry.Rotation2d; @@ -31,6 +32,12 @@ public class Module { private final Alert driveDisconnectedAlert; private final Alert turnDisconnectedAlert; + private final Alert turnEncoderDisconnectedAlert; + private final Alert driveInitFailedAlert; + private final Alert turnConfigFailedAlert; + private final Alert turnEncoderConfigFailedAlert; + private final Alert driveFirmwareBlockedAlert; + private final Alert turnFirmwareBlockedAlert; private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; public Module(ModuleIO io, String name) { @@ -47,6 +54,19 @@ public Module(ModuleIO io, String name) { "Module/" + name + "/turnDisconnected", "Disconnected turn motor on module " + name + ".", Alert.Level.HIGH); + turnEncoderDisconnectedAlert = + new Alert( + "Module/" + name + "/turnEncoderDisconnected", + "Disconnected turn encoder on module " + name + ".", + Alert.Level.HIGH); + driveInitFailedAlert = new Alert("Module/" + name + "/driveInitFailed", "", Alert.Level.HIGH); + turnConfigFailedAlert = new Alert("Module/" + name + "/turnConfigFailed", "", Alert.Level.HIGH); + turnEncoderConfigFailedAlert = + new Alert("Module/" + name + "/turnEncoderConfigFailed", "", Alert.Level.HIGH); + driveFirmwareBlockedAlert = + new Alert("Module/" + name + "/driveFirmwareBlocked", "", Alert.Level.HIGH); + turnFirmwareBlockedAlert = + new Alert("Module/" + name + "/turnFirmwareBlocked", "", Alert.Level.HIGH); } public void periodic() { @@ -57,9 +77,13 @@ public void periodic() { long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; if (!encoderInitialized) { - // Set turn zero from preferences + // Set turn zero from preferences. The CANcoder's zero seeds a missing preference only when + // its config was read. After a failed read the zero is a default, and saving it would + // replace the module's calibration. Rotation2d turnZeroFromCancoder = inputs.turnZero; - Preferences.initDouble(ZERO_ROTATION_KEY + "/" + name, turnZeroFromCancoder.getRadians()); + if ("OK".equals(inputs.turnEncoderRefreshStatus)) { + Preferences.initDouble(ZERO_ROTATION_KEY + "/" + name, turnZeroFromCancoder.getRadians()); + } Rotation2d turnZeroFromPreferences = new Rotation2d( Preferences.getDouble( @@ -82,6 +106,30 @@ public void periodic() { // Update alerts driveDisconnectedAlert.set(!inputs.driveConnected); turnDisconnectedAlert.set(!inputs.turnConnected); + turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); + setStatusAlert( + driveInitFailedAlert, + inputs.driveInitStatus, + "Drive motor setup failed on module " + name + " (" + inputs.driveInitStatus + ")."); + setStatusAlert( + turnConfigFailedAlert, + inputs.turnConfigStatus, + "Turn motor config failed on module " + name + " (" + inputs.turnConfigStatus + ")."); + boolean turnEncoderReadOk = "OK".equals(inputs.turnEncoderRefreshStatus); + String turnEncoderStatus = + turnEncoderReadOk ? inputs.turnEncoderApplyStatus : inputs.turnEncoderRefreshStatus; + setStatusAlert( + turnEncoderConfigFailedAlert, + turnEncoderStatus, + turnEncoderReadOk + ? "Turn encoder config write failed on module " + name + " (" + turnEncoderStatus + ")." + : "Turn encoder config read failed on module " + + name + + " (" + + turnEncoderStatus + + "); turn zero not saved."); + setBlockedAlert(driveFirmwareBlockedAlert, inputs.driveControlStatus, "drive motor"); + setBlockedAlert(turnFirmwareBlockedAlert, inputs.turnControlStatus, "turn motor"); Logger.recordOutput("Faults/Module" + name + "/DriveDisconnected", !inputs.driveConnected); Logger.recordOutput("Faults/Module" + name + "/TurnDisconnected", !inputs.turnConnected); long t3 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; @@ -104,6 +152,30 @@ public void periodic() { } } + /** Activates the alert when the Phoenix status isn't "OK", with text naming the status. */ + private static void setStatusAlert(Alert alert, String status, String text) { + boolean failed = !"OK".equals(status); + if (failed && !text.equals(alert.getText())) alert.setText(text); + alert.set(failed); + } + + /** Activates the alert when the setControl status shows Phoenix blocking the motor's output. */ + private void setBlockedAlert(Alert alert, String controlStatus, String device) { + boolean blocked = PhoenixFirmware.isBlocked(controlStatus); + if (blocked) { + String text = + "Phoenix is blocking " + + device + + " output on module " + + name + + ": " + + controlStatus + + ". Update the motor firmware or the Phoenix library."; + if (!text.equals(alert.getText())) alert.setText(text); + } + alert.set(blocked); + } + /** Runs the module with the specified setpoint state. */ public void runSetpoint(SwerveModuleVelocity state) { runSetpoint(state, Translation2d.ZERO); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 45144fc..491e0fa 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -31,6 +31,21 @@ public static class ModuleIOInputs { public double[] odometryTimestamps = new double[] {}; public double[] odometryDrivePositionsRad = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + + // Phoenix StatusCode names from device setup. "OK" means no known failure. + public String driveInitStatus = "OK"; + public String turnConfigStatus = "OK"; + public String turnEncoderRefreshStatus = "OK"; + public String turnEncoderApplyStatus = "OK"; + + // Phoenix StatusCode names returned by the most recent setControl call on each motor + public String driveControlStatus = "OK"; + public String turnControlStatus = "OK"; + + // Firmware versions as "major.minor.bugfix.build", or "" when unknown + public String driveFirmware = ""; + public String turnFirmware = ""; + public String turnEncoderFirmware = ""; } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java index 70ce1c6..f619806 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java @@ -13,6 +13,7 @@ import static frc.robot.util.odometry.PhoenixUtil.*; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -27,6 +28,7 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import frc.lib.hardware.PhoenixFirmware; import frc.robot.Constants.CANBusPorts.SC1; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.util.Units; @@ -66,6 +68,20 @@ public abstract class ModuleIOTalonFXBase implements ModuleIO { protected final StatusSignal turnVelocity; protected final StatusSignal turnAppliedVolts; protected final StatusSignal turnCurrent; + private final StatusSignal driveVersion; + private final StatusSignal turnVersion; + private final StatusSignal turnEncoderVersion; + + // Setup results. The encoder refresh result stays fixed after construction: a failed refresh + // means cancoderConfig holds defaults, so its magnet offset is not the device's real zero. + private final StatusCode driveInitStatus; + private final StatusCode turnConfigStatus; + private final StatusCode turnEncoderRefreshStatus; + private StatusCode turnEncoderApplyStatus = StatusCode.OK; + + // Results of the most recent setControl calls, which report when Phoenix blocks output + private StatusCode driveControlStatus = StatusCode.OK; + private StatusCode turnControlStatus = StatusCode.OK; protected ModuleIOTalonFXBase( SwerveModuleConstants @@ -75,18 +91,33 @@ protected ModuleIOTalonFXBase( turnTalon = new TalonFX(constants.SteerMotorId, SC1.BUS); cancoder = new CANcoder(constants.EncoderId, SC1.BUS); - tryUntilOk( - 5, () -> driveTalon.getConfigurator().apply(constants.DriveMotorInitialConfigs, 0.25)); - tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); - tryUntilOk( - 5, () -> turnTalon.getConfigurator().apply(constants.SteerMotorInitialConfigs, 0.25)); - - cancoder.getConfigurator().refresh(cancoderConfig); + driveInitStatus = + firstError( + tryUntilOk( + 5, + () -> driveTalon.getConfigurator().apply(constants.DriveMotorInitialConfigs, 0.25)), + tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25))); + turnConfigStatus = + tryUntilOk( + 5, () -> turnTalon.getConfigurator().apply(constants.SteerMotorInitialConfigs, 0.25)); + + // Read the CANcoder's config first so the apply keeps settings made in Phoenix Tuner. If the + // read fails, skip the apply rather than write defaults over the device. + turnEncoderRefreshStatus = + tryUntilOk(5, () -> cancoder.getConfigurator().refresh(cancoderConfig)); cancoderConfig.MagnetSensor.SensorDirection = constants.EncoderInverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; - cancoder.getConfigurator().apply(cancoderConfig); + if (turnEncoderRefreshStatus.isOK()) { + turnEncoderApplyStatus = + tryUntilOk(5, () -> cancoder.getConfigurator().apply(cancoderConfig)); + } + + // getVersion() refreshes on each call, so keep the signals and refresh them with the others + driveVersion = driveTalon.getVersion(); + turnVersion = turnTalon.getVersion(); + turnEncoderVersion = cancoder.getVersion(); drivePosition = driveTalon.getPosition(); driveVelocity = driveTalon.getVelocity(); @@ -110,7 +141,10 @@ protected void readSignalInputs(ModuleIOInputs inputs) { turnPosition, turnVelocity, turnAppliedVolts, - turnCurrent); + turnCurrent, + driveVersion, + turnVersion, + turnEncoderVersion); inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); @@ -123,24 +157,42 @@ protected void readSignalInputs(ModuleIOInputs inputs) { inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + + inputs.driveInitStatus = driveInitStatus.getName(); + inputs.turnConfigStatus = turnConfigStatus.getName(); + inputs.turnEncoderRefreshStatus = turnEncoderRefreshStatus.getName(); + inputs.turnEncoderApplyStatus = turnEncoderApplyStatus.getName(); + inputs.driveControlStatus = driveControlStatus.getName(); + inputs.turnControlStatus = turnControlStatus.getName(); + + inputs.driveFirmware = firmwareVersion(driveVersion); + inputs.turnFirmware = firmwareVersion(turnVersion); + inputs.turnEncoderFirmware = firmwareVersion(turnEncoderVersion); + } + + /** Returns the signal's firmware version as "major.minor.bugfix.build", or "" when unknown. */ + private static String firmwareVersion(StatusSignal version) { + return version.getStatus().isOK() ? PhoenixFirmware.format(version.getValue()) : ""; } @Override public void setDriveOpenLoop(double output) { - driveTalon.setControl( - switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(output); - case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); - }); + driveControlStatus = + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); } @Override public void setTurnOpenLoop(double output) { - turnTalon.setControl( - switch (constants.SteerMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(output); - case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); - }); + turnControlStatus = + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); } @Override @@ -151,15 +203,18 @@ public void setDriveVelocity(double velocityRadPerSec, double feedforwardWheelTo constants.DriveMotorClosedLoopOutput, feedforwardWheelTorqueNm, constants.DriveMotorGearRatio); - driveTalon.setControl( - switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> - velocityVoltageRequest.withVelocity(velocityRotPerSec).withFeedForward(feedforward); - case TorqueCurrentFOC -> - velocityTorqueCurrentRequest - .withVelocity(velocityRotPerSec) - .withFeedForward(feedforward); - }); + driveControlStatus = + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> + velocityVoltageRequest + .withVelocity(velocityRotPerSec) + .withFeedForward(feedforward); + case TorqueCurrentFOC -> + velocityTorqueCurrentRequest + .withVelocity(velocityRotPerSec) + .withFeedForward(feedforward); + }); } /** @@ -181,17 +236,19 @@ static double driveFeedforward( @Override public void setTurnPosition(Rotation2d rotation) { - turnTalon.setControl( - switch (constants.SteerMotorClosedLoopOutput) { - case Voltage -> motionMagicVoltageRequest.withPosition(rotation.getRotations()); - case TorqueCurrentFOC -> - motionMagicTorqueCurrentRequest.withPosition(rotation.getRotations()); - }); + turnControlStatus = + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> motionMagicVoltageRequest.withPosition(rotation.getRotations()); + case TorqueCurrentFOC -> + motionMagicTorqueCurrentRequest.withPosition(rotation.getRotations()); + }); } @Override public void setTurnZero(Rotation2d rotation) { cancoderConfig.MagnetSensor.MagnetOffset = rotation.getRotations(); - cancoder.getConfigurator().apply(cancoderConfig); + // One attempt only: this runs on the main loop, where retries would stall the robot + turnEncoderApplyStatus = cancoder.getConfigurator().apply(cancoderConfig); } } diff --git a/src/main/java/frc/robot/util/odometry/PhoenixUtil.java b/src/main/java/frc/robot/util/odometry/PhoenixUtil.java index 5fad52e..1d5962c 100644 --- a/src/main/java/frc/robot/util/odometry/PhoenixUtil.java +++ b/src/main/java/frc/robot/util/odometry/PhoenixUtil.java @@ -11,11 +11,25 @@ import java.util.function.Supplier; public class PhoenixUtil { - /** Attempts to run the command until no error is produced. */ - public static void tryUntilOk(int maxAttempts, Supplier command) { + /** + * Attempts to run the command until no error is produced. + * + * @return the status of the last attempt: OK on success, otherwise the final error + */ + public static StatusCode tryUntilOk(int maxAttempts, Supplier command) { + StatusCode status = StatusCode.OK; for (int i = 0; i < maxAttempts; i++) { - var error = command.get(); - if (error.isOK()) break; + status = command.get(); + if (status.isOK()) break; } + return status; + } + + /** Returns the first status that is not OK, or OK if every status is OK. */ + public static StatusCode firstError(StatusCode... statuses) { + for (StatusCode status : statuses) { + if (!status.isOK()) return status; + } + return StatusCode.OK; } } diff --git a/src/test/java/frc/lib/hardware/CANBusHealthTest.java b/src/test/java/frc/lib/hardware/CANBusHealthTest.java new file mode 100644 index 0000000..962abf3 --- /dev/null +++ b/src/test/java/frc/lib/hardware/CANBusHealthTest.java @@ -0,0 +1,128 @@ +// Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.lib.hardware; + +import static org.junit.jupiter.api.Assertions.*; + +import frc.lib.hardware.CANBusHealth.Severity; +import org.junit.jupiter.api.Test; + +class CANBusHealthTest { + private static Severity severity(String status, String state) { + return CANBusHealth.severity(status, state, false, false, false); + } + + @Test + void healthyBusIsNone() { + assertEquals(Severity.NONE, severity("OK", "ErrorActive")); + } + + @Test + void errorWarningIsMedium() { + assertEquals(Severity.MEDIUM, severity("OK", "ErrorWarning")); + } + + @Test + void faultedStatesAreHigh() { + assertEquals(Severity.HIGH, severity("OK", "ErrorPassive")); + assertEquals(Severity.HIGH, severity("OK", "BusOff")); + assertEquals(Severity.HIGH, severity("OK", "Stopped")); + } + + @Test + void failedStatusReadIsHigh() { + assertEquals(Severity.HIGH, severity("InvalidNetwork", "ErrorActive")); + } + + @Test + void risingCountersAndStaleAreHigh() { + assertEquals(Severity.HIGH, CANBusHealth.severity("OK", "ErrorActive", true, false, false)); + assertEquals(Severity.HIGH, CANBusHealth.severity("OK", "ErrorActive", false, true, false)); + assertEquals(Severity.HIGH, CANBusHealth.severity("OK", "ErrorActive", false, false, true)); + } + + @Test + void noSamplesNeverGoStale() { + var health = new CANBusHealth(); + for (double t = 0; t < 10; t += 0.02) { + health.update(t, 0, "OK", "ErrorActive", 0, 0); + assertFalse(health.isHighActive(t)); + } + } + + @Test + void firstSampleWithOldCountsIsNotARise() { + var health = new CANBusHealth(); + health.update(0.0, 1, "OK", "ErrorActive", 7, 3); + assertFalse(health.isHighActive(0.0)); + } + + @Test + void busOffIncreaseAfterFirstSampleIsHigh() { + var health = new CANBusHealth(); + health.update(0.0, 1, "OK", "ErrorActive", 7, 3); + health.update(0.4, 2, "OK", "ErrorActive", 8, 3); + assertTrue(health.isHighActive(0.4)); + } + + @Test + void restartIncreaseAfterFirstSampleIsHigh() { + var health = new CANBusHealth(); + health.update(0.0, 1, "OK", "ErrorActive", 0, 3); + health.update(0.4, 2, "OK", "ErrorActive", 0, 4); + assertTrue(health.isHighActive(0.4)); + } + + @Test + void stalledReaderGoesStale() { + var health = new CANBusHealth(); + health.update(0.0, 1, "OK", "ErrorActive", 0, 0); + health.update(1.0, 1, "OK", "ErrorActive", 0, 0); + assertFalse(health.isHighActive(1.0)); + health.update(1.6, 1, "OK", "ErrorActive", 0, 0); + assertTrue(health.isHighActive(1.6)); + } + + @Test + void advancingReaderStaysFresh() { + var health = new CANBusHealth(); + long count = 1; + for (double t = 0; t < 5; t += 0.02) { + if (t >= count * 0.4) count++; + health.update(t, count, "OK", "ErrorActive", 0, 0); + assertFalse(health.isHighActive(t)); + } + } + + @Test + void highHoldsForHalfASecondAfterLastError() { + var health = new CANBusHealth(); + health.update(0.0, 1, "OK", "BusOff", 0, 0); + health.update(0.02, 1, "OK", "ErrorActive", 0, 0); + assertTrue(health.isHighActive(0.49)); + assertFalse(health.isHighActive(0.5)); + } + + @Test + void mediumHoldsForHalfASecondAfterLastWarning() { + var health = new CANBusHealth(); + health.update(0.0, 1, "OK", "ErrorWarning", 0, 0); + assertTrue(health.isMediumActive(0.0)); + assertFalse(health.isHighActive(0.0)); + health.update(0.02, 1, "OK", "ErrorActive", 0, 0); + assertTrue(health.isMediumActive(0.49)); + assertFalse(health.isMediumActive(0.5)); + } + + @Test + void startsInactive() { + var health = new CANBusHealth(); + assertFalse(health.isHighActive(0.0)); + assertFalse(health.isMediumActive(0.0)); + } +} diff --git a/src/test/java/frc/lib/hardware/PhoenixFirmwareTest.java b/src/test/java/frc/lib/hardware/PhoenixFirmwareTest.java new file mode 100644 index 0000000..deeb8b4 --- /dev/null +++ b/src/test/java/frc/lib/hardware/PhoenixFirmwareTest.java @@ -0,0 +1,48 @@ +// Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.lib.hardware; + +import static org.junit.jupiter.api.Assertions.*; + +import org.junit.jupiter.api.Test; + +class PhoenixFirmwareTest { + @Test + void firmwareTooOldIsBlocked() { + assertTrue(PhoenixFirmware.isBlocked("FirmwareTooOld")); + } + + @Test + void apiTooOldIsBlocked() { + assertTrue(PhoenixFirmware.isBlocked("ApiTooOld")); + } + + @Test + void otherStatusesAreNotBlocked() { + assertFalse(PhoenixFirmware.isBlocked("OK")); + assertFalse(PhoenixFirmware.isBlocked("TxFailed")); + assertFalse(PhoenixFirmware.isBlocked("")); + assertFalse(PhoenixFirmware.isBlocked(null)); + } + + @Test + void formatsMostSignificantByteFirst() { + assertEquals("26.2.3.0", PhoenixFirmware.format(0x1A020300)); + } + + @Test + void formatsBytesAsUnsigned() { + assertEquals("255.0.0.0", PhoenixFirmware.format(0xFF000000)); + assertEquals("128.255.128.255", PhoenixFirmware.format(0x80FF80FF)); + } + + @Test + void zeroMeansUnknown() { + assertEquals("", PhoenixFirmware.format(0)); + } +} diff --git a/src/test/java/frc/robot/util/odometry/PhoenixUtilTest.java b/src/test/java/frc/robot/util/odometry/PhoenixUtilTest.java new file mode 100644 index 0000000..639a580 --- /dev/null +++ b/src/test/java/frc/robot/util/odometry/PhoenixUtilTest.java @@ -0,0 +1,51 @@ +// Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.util.odometry; + +import static org.junit.jupiter.api.Assertions.*; + +import com.ctre.phoenix6.StatusCode; +import org.junit.jupiter.api.Test; + +class PhoenixUtilTest { + @Test + void returnsOkAfterFirstSuccess() { + int[] calls = {0}; + StatusCode status = + PhoenixUtil.tryUntilOk( + 5, + () -> { + calls[0]++; + return calls[0] < 3 ? StatusCode.ConfigFailed : StatusCode.OK; + }); + assertEquals(StatusCode.OK, status); + assertEquals(3, calls[0]); + } + + @Test + void returnsLastErrorWhenEveryAttemptFails() { + int[] calls = {0}; + StatusCode status = + PhoenixUtil.tryUntilOk( + 5, + () -> { + calls[0]++; + return calls[0] < 5 ? StatusCode.ConfigFailed : StatusCode.TxFailed; + }); + assertEquals(StatusCode.TxFailed, status); + assertEquals(5, calls[0]); + } + + @Test + void firstErrorPicksTheFirstFailure() { + assertEquals( + StatusCode.TxFailed, + PhoenixUtil.firstError(StatusCode.OK, StatusCode.TxFailed, StatusCode.ConfigFailed)); + assertEquals(StatusCode.OK, PhoenixUtil.firstError(StatusCode.OK, StatusCode.OK)); + } +}