feat: refresh all signals at the same time (scuffed implementation?)

Author: mimizh2418

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

@@ -9,6 +9,7 @@
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -83,6 +84,7 @@ public void robotInit() {
      */
     @Override
     public void robotPeriodic() {
+        if (Constants.currentMode == Constants.Mode.REAL) PhoenixTimeSyncSignalRefresher.refreshSignals();
         // 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.

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

@@ -1,12 +1,12 @@
 package org.team1540.robot2024.subsystems.drive;
 
 import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
+import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
 
 /**
  * IO implementation for Pigeon2
@@ -22,11 +22,13 @@ public GyroIOPigeon2() {
         yaw.setUpdateFrequency(100.0);
         yawVelocity.setUpdateFrequency(100.0);
         pigeon.optimizeBusUtilization();
+
+        PhoenixTimeSyncSignalRefresher.registerSignals(yaw, yawVelocity);
     }
 
     @Override
     public void updateInputs(GyroIOInputs inputs) {
-        inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
+        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

@@ -1,7 +1,6 @@
 package org.team1540.robot2024.subsystems.drive;
 
 import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.controls.VoltageOut;
@@ -11,10 +10,10 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
+import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
 import org.team1540.robot2024.util.swerve.SwerveFactory;
 
 import static org.team1540.robot2024.Constants.Drivetrain.*;
-import static org.team1540.robot2024.Constants.SwerveConfig.CAN_BUS;
 
 /**
  * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and
@@ -44,15 +43,11 @@ public class ModuleIOTalonFX implements ModuleIO {
     private final StatusSignal<Double> turnAppliedVolts;
     private final StatusSignal<Double> turnCurrent;
 
-    private final boolean isCANFD;
-
     public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) {
         driveTalon = hw.driveMotor;
         turnTalon = hw.turnMotor;
         cancoder = hw.cancoder;
 
-        isCANFD = CANBus.isNetworkFD(CAN_BUS);
-
         setDriveBrakeMode(true);
         setTurnBrakeMode(true);
 
@@ -79,32 +74,20 @@ public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) {
                 turnCurrent);
         driveTalon.optimizeBusUtilization();
         turnTalon.optimizeBusUtilization();
+
+        PhoenixTimeSyncSignalRefresher.registerSignals(drivePosition, turnPosition);
     }
 
     @Override
     public void updateInputs(ModuleIOInputs inputs) {
-        if (isCANFD) {
-            BaseStatusSignal.waitForAll(0.01, drivePosition, turnPosition);
-            BaseStatusSignal.refreshAll(
-                    driveVelocity,
-                    driveAppliedVolts,
-                    driveCurrent,
-                    turnAbsolutePosition,
-                    turnVelocity,
-                    turnAppliedVolts,
-                    turnCurrent);
-        } else {
-            BaseStatusSignal.refreshAll(
-                    drivePosition,
-                    driveVelocity,
-                    driveAppliedVolts,
-                    driveCurrent,
-                    turnAbsolutePosition,
-                    turnPosition,
-                    turnVelocity,
-                    turnAppliedVolts,
-                    turnCurrent);
-        }
+        BaseStatusSignal.refreshAll(
+                driveVelocity,
+                driveAppliedVolts,
+                driveCurrent,
+                turnAbsolutePosition,
+                turnVelocity,
+                turnAppliedVolts,
+                turnCurrent);
 
         inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO;
         inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO;

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

@@ -0,0 +1,26 @@
+package org.team1540.robot2024.util;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
+
+import java.util.Arrays;
+
+import static org.team1540.robot2024.Constants.SwerveConfig.*;
+
+/**
+ * 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);
+
+    public static void registerSignals(BaseStatusSignal... newSignals) {
+        signals = Arrays.copyOf(signals, signals.length + newSignals.length);
+        for (int i = 0; i < newSignals.length; i++) signals[signals.length - 1 - i] = newSignals[i];
+    }
+
+    public static void refreshSignals() {
+        if (isCANFD) BaseStatusSignal.waitForAll(0.01, signals);
+        else BaseStatusSignal.refreshAll(signals);
+    }
+}