fix: potentially less scuffed signal refresher

Author: mimizh2418

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

@@ -9,7 +9,6 @@
 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
@@ -84,7 +83,6 @@ 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

@@ -30,6 +30,7 @@ public GyroIOPigeon2() {
 
     @Override
     public void updateInputs(GyroIOInputs inputs) {
+        PhoenixTimeSyncSignalRefresher.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

@@ -80,6 +80,7 @@ public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) {
 
     @Override
     public void updateInputs(ModuleIOInputs inputs) {
+        PhoenixTimeSyncSignalRefresher.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 edu.wpi.first.wpilibj.Timer;
+import org.team1540.robot2024.Constants;
 
 import java.util.Arrays;
 
@@ -14,12 +16,22 @@ public class PhoenixTimeSyncSignalRefresher {
     private static BaseStatusSignal[] signals = new BaseStatusSignal[0];
     private static final boolean isCANFD = CANBus.isNetworkFD(CAN_BUS);
 
+    private static double lastRefreshTimestamp = 0;
+
+    /**
+     * Registers status signals to be refreshed when {@link PhoenixTimeSyncSignalRefresher#refreshSignals()}is called
+     */
     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];
     }
 
+    /**
+     * Refreshes all registered signals if not already done so in the current loop period
+     */
     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);
     }