fix+feat: signal refresher no longer static, better periodic updating…

…, and error checking

src/main/java/org/team1540/robot2024/Robot.java

@@ -89,6 +89,7 @@ public void robotPeriodic() {
         // This must be called from the robot's periodic block in order for anything in
         // the Command-based framework to work.
         CommandScheduler.getInstance().run();
+        if (Constants.currentMode == Constants.Mode.REAL) robotContainer.timeSyncSignalRefresher.periodic();
     }
 
     /**

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -14,6 +14,7 @@
 import org.team1540.robot2024.commands.SwerveDriveCommand;
 import org.team1540.robot2024.subsystems.drive.*;
 import org.team1540.robot2024.subsystems.shooter.*;
+import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
 import org.team1540.robot2024.util.swerve.SwerveFactory;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -37,6 +38,8 @@ public class RobotContainer {
     // Dashboard inputs
     public final LoggedDashboardChooser<Command> autoChooser;
 
+    public final PhoenixTimeSyncSignalRefresher timeSyncSignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
+
     // TODO: testing dashboard inputs, remove for comp
     public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
     public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);
@@ -50,11 +53,11 @@ public RobotContainer() {
                 // Real robot, instantiate hardware IO implementations
                 drivetrain =
                         new Drivetrain(
-                                new GyroIOPigeon2(),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT)),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
+                                new GyroIOPigeon2(timeSyncSignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), timeSyncSignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), timeSyncSignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), timeSyncSignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), timeSyncSignalRefresher));
                 shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
                 break;
 

src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java

@@ -18,19 +18,22 @@ public class GyroIOPigeon2 implements GyroIO {
     private final StatusSignal<Double> yaw = pigeon.getYaw();
     private final StatusSignal<Double> yawVelocity = pigeon.getAngularVelocityZDevice();
 
-    public GyroIOPigeon2() {
+    private final PhoenixTimeSyncSignalRefresher timeSyncSignalRefresher;
+
+    public GyroIOPigeon2(PhoenixTimeSyncSignalRefresher timeSyncSignalRefresher) {
         pigeon.getConfigurator().apply(new Pigeon2Configuration());
         pigeon.getConfigurator().setYaw(0.0);
         yaw.setUpdateFrequency(100.0);
         yawVelocity.setUpdateFrequency(100.0);
         pigeon.optimizeBusUtilization();
 
-        PhoenixTimeSyncSignalRefresher.registerSignals(yaw, yawVelocity);
+        this.timeSyncSignalRefresher = timeSyncSignalRefresher;
+        timeSyncSignalRefresher.registerSignals(yaw, yawVelocity);
     }
 
     @Override
     public void updateInputs(GyroIOInputs inputs) {
-        PhoenixTimeSyncSignalRefresher.refreshSignals();
+        timeSyncSignalRefresher.refreshSignals();
         inputs.connected = BaseStatusSignal.isAllGood(yaw, yawVelocity);
         inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
         inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());

src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java

@@ -43,7 +43,9 @@ public class ModuleIOTalonFX implements ModuleIO {
     private final StatusSignal<Double> turnAppliedVolts;
     private final StatusSignal<Double> turnCurrent;
 
-    public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) {
+    private final PhoenixTimeSyncSignalRefresher timeSyncSignalRefresher;
+
+    public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw, PhoenixTimeSyncSignalRefresher timeSyncSignalRefresher) {
         driveTalon = hw.driveMotor;
         turnTalon = hw.turnMotor;
         cancoder = hw.cancoder;
@@ -75,12 +77,14 @@ public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) {
         driveTalon.optimizeBusUtilization();
         turnTalon.optimizeBusUtilization();
 
-        PhoenixTimeSyncSignalRefresher.registerSignals(drivePosition, turnPosition);
+        this.timeSyncSignalRefresher = timeSyncSignalRefresher;
+
+        timeSyncSignalRefresher.registerSignals(drivePosition, turnPosition);
     }
 
     @Override
     public void updateInputs(ModuleIOInputs inputs) {
-        PhoenixTimeSyncSignalRefresher.refreshSignals();
+        timeSyncSignalRefresher.refreshSignals();
         BaseStatusSignal.refreshAll(
                 driveVelocity,
                 driveAppliedVolts,

src/main/java/org/team1540/robot2024/util/PhoenixTimeSyncSignalRefresher.java

@@ -2,6 +2,8 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.StatusCode;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import org.team1540.robot2024.Constants;
 
@@ -13,26 +15,56 @@
  * Helper class for refreshing CANivore TimeSynced status signals.
  */
 public class PhoenixTimeSyncSignalRefresher {
-    private static BaseStatusSignal[] signals = new BaseStatusSignal[0];
-    private static final boolean isCANFD = CANBus.isNetworkFD(CAN_BUS);
+    private BaseStatusSignal[] signals = new BaseStatusSignal[0];
+    private double refreshFrequencyHz = -1;
+    private boolean useTimeSync;
+    private boolean isRefreshed;
 
-    private static double lastRefreshTimestamp = 0;
+    public PhoenixTimeSyncSignalRefresher(String canbus) {
+        useTimeSync = CANBus.isNetworkFD(canbus);
+    }
 
     /**
-     * Registers status signals to be refreshed when {@link PhoenixTimeSyncSignalRefresher#refreshSignals()}is called
+     * Registers status signals to be refreshed when {@link PhoenixTimeSyncSignalRefresher#refreshSignals()} is called
      */
-    public static void registerSignals(BaseStatusSignal... newSignals) {
+    public void registerSignals(BaseStatusSignal... newSignals) {
+        if (newSignals.length < 1) return;
+        if (refreshFrequencyHz < 0) refreshFrequencyHz = newSignals[0].getAppliedUpdateFrequency();
+
         signals = Arrays.copyOf(signals, signals.length + newSignals.length);
-        for (int i = 0; i < newSignals.length; i++) signals[signals.length - 1 - i] = newSignals[i];
+        for (int i = 0; i < newSignals.length; i++) {
+            // Check if we have mismatched update frequencies
+            if (newSignals[i].getAppliedUpdateFrequency() != refreshFrequencyHz && useTimeSync) {
+                useTimeSync = false;
+                DriverStation.reportWarning(
+                        "Status signals received have varying update rates, TimeSynced updates are now unavailable",
+                        true);
+            }
+            signals[signals.length - 1 - i] = newSignals[i];
+        }
     }
 
     /**
-     * Refreshes all registered signals if not already done so in the current loop period
+     * Refreshes all registered signals if not done so since {@link PhoenixTimeSyncSignalRefresher#periodic()} call
      */
-    public static void refreshSignals() {
-        if (Timer.getFPGATimestamp() - lastRefreshTimestamp < Constants.LOOP_PERIOD_SECS) return;
-        lastRefreshTimestamp = Timer.getFPGATimestamp();
-        if (isCANFD) BaseStatusSignal.waitForAll(0.01, signals);
-        else BaseStatusSignal.refreshAll(signals);
+    public void refreshSignals() {
+        if (isRefreshed) return;
+
+        // Refresh signals
+        StatusCode status;
+        if (useTimeSync) status = BaseStatusSignal.waitForAll(0.01, signals);
+        else status = BaseStatusSignal.refreshAll(signals);
+
+        // Check errors
+        if (status == StatusCode.InvalidNetwork)
+            DriverStation.reportError("One or more status signals passed to signal refresher has a different CAN bus, unable to refresh", true);
+        if (status == StatusCode.RxTimeout)
+            DriverStation.reportWarning("Refreshing signals timed out, check CAN bus wiring", true);
+
+        isRefreshed = true;
+    }
+
+    public void periodic() {
+        isRefreshed = false;
     }
 }