Use signalmanager to make only one JNI call for all CTRE signals

src/main/java/com/techhounds/houndutil/houndlib/robots/HoundRobot.java

@@ -1,5 +1,6 @@
 package com.techhounds.houndutil.houndlib.robots;
 
+import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -9,7 +10,9 @@
 
 import com.techhounds.houndutil.houndauto.AutoManager;
 import com.techhounds.houndutil.houndlib.TriConsumer;
+import com.techhounds.houndutil.houndlog.FaultLogger;
 import com.techhounds.houndutil.houndlog.LoggingManager;
+import com.techhounds.houndutil.houndlog.SignalManager;
 
 public class HoundRobot extends TimedRobot {
     public HoundRobot() {
@@ -34,14 +37,16 @@ public void robotInit() {
         // sets the LoggingManager to run every 100ms and on an offset of 10ms from the
         // main thread
         LoggingManager.getInstance().init();
-        addPeriodic(LoggingManager.getInstance()::run, 0.100, 0.010);
+        addPeriodic(FaultLogger::update, 0.100, 0.010);
         LiveWindow.disableAllTelemetry();
+        SignalManager.registerAll();
+        Threads.setCurrentThreadPriority(true, 99);
     }
 
     @Override
     public void robotPeriodic() {
         CommandScheduler.getInstance().run();
-        // LoggingManager.getInstance().run();
+        LoggingManager.getInstance().run();
     }
 
     @Override

src/main/java/com/techhounds/houndutil/houndlib/swerve/KrakenCoaxialSwerveModule.java

@@ -1,6 +1,7 @@
 package com.techhounds.houndutil.houndlib.swerve;
 
 import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MagnetSensorConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
@@ -17,6 +18,7 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.techhounds.houndutil.houndlib.MotorHoldMode;
+import com.techhounds.houndutil.houndlog.SignalManager;
 import com.techhounds.houndutil.houndlog.interfaces.Log;
 import com.techhounds.houndutil.houndlog.interfaces.LoggedObject;
 
@@ -50,6 +52,15 @@ public class KrakenCoaxialSwerveModule implements CoaxialSwerveModule {
 
     private SwerveModuleState previousState = new SwerveModuleState();
 
+    private final StatusSignal<Double> drivePosition;
+    private final StatusSignal<Double> driveVelocity;
+    private final StatusSignal<Double> driveAcceleration;
+    private final StatusSignal<Double> driveMotorVoltage;
+    private final StatusSignal<Double> steerPosition;
+    private final StatusSignal<Double> steerVelocity;
+    private final StatusSignal<Double> steerAcceleration;
+    private final StatusSignal<Double> steerMotorVoltage;
+
     /**
      * Initalizes a SwerveModule.
      *
@@ -137,30 +148,37 @@ public KrakenCoaxialSwerveModule(
                 SWERVE_CONSTANTS.STEER_GEARING,
                 SWERVE_CONSTANTS.STEER_MOI);
 
+        drivePosition = driveMotor.getPosition();
+        driveVelocity = driveMotor.getVelocity();
+        driveAcceleration = driveMotor.getAcceleration();
+        driveMotorVoltage = driveMotor.getMotorVoltage();
+        steerPosition = steerMotor.getPosition();
+        steerVelocity = steerMotor.getVelocity();
+        steerAcceleration = steerMotor.getAcceleration();
+        steerMotorVoltage = steerMotor.getMotorVoltage();
+
         BaseStatusSignal.setUpdateFrequencyForAll(250,
-                driveMotor.getPosition(),
-                driveMotor.getVelocity(),
-                driveMotor.getAcceleration(),
-                driveMotor.getMotorVoltage(),
-                steerMotor.getPosition(),
-                steerMotor.getVelocity(),
-                steerMotor.getAcceleration(),
-                steerMotor.getMotorVoltage());
+                drivePosition, driveVelocity, driveAcceleration, driveMotorVoltage,
+                steerPosition, steerVelocity, steerAcceleration, steerMotorVoltage);
+
+        SignalManager.register(
+                drivePosition, driveVelocity, driveAcceleration, driveMotorVoltage,
+                steerPosition, steerVelocity, steerAcceleration, steerMotorVoltage);
     }
 
     @Override
     public double getDriveMotorPosition() {
-        return BaseStatusSignal.getLatencyCompensatedValue(driveMotor.getPosition(), driveMotor.getVelocity());
+        return BaseStatusSignal.getLatencyCompensatedValue(drivePosition, driveVelocity);
     }
 
     @Override
     public double getDriveMotorVelocity() {
-        return BaseStatusSignal.getLatencyCompensatedValue(driveMotor.getVelocity(), driveMotor.getAcceleration());
+        return BaseStatusSignal.getLatencyCompensatedValue(driveVelocity, driveAcceleration);
     }
 
     @Override
     public double getDriveMotorVoltage() {
-        return driveMotor.getMotorVoltage().getValue();
+        return driveMotorVoltage.getValue();
     }
 
     public TalonFX getDriveMotor() {
@@ -169,26 +187,25 @@ public TalonFX getDriveMotor() {
 
     @Override
     public double getSteerMotorPosition() {
-        return BaseStatusSignal.getLatencyCompensatedValue(steerMotor.getPosition(), steerMotor.getVelocity());
+        return BaseStatusSignal.getLatencyCompensatedValue(steerPosition, steerVelocity);
     }
 
     @Override
     public double getSteerMotorVelocity() {
-        return BaseStatusSignal.getLatencyCompensatedValue(steerMotor.getVelocity(), steerMotor.getAcceleration());
+        return BaseStatusSignal.getLatencyCompensatedValue(steerVelocity, steerAcceleration);
     }
 
     @Override
     public double getSteerMotorVoltage() {
-        return steerMotor.getMotorVoltage().getValue();
+        return steerMotorVoltage.getValue();
     }
 
     public TalonFX getSteerMotor() {
         return steerMotor;
     }
 
     public BaseStatusSignal[] getSignals() {
-        return new BaseStatusSignal[] { driveMotor.getPosition(), driveMotor.getVelocity(),
-                steerMotor.getPosition(), steerMotor.getVelocity() };
+        return new BaseStatusSignal[] { drivePosition, driveVelocity, steerPosition, steerVelocity };
     }
 
     @Override

src/main/java/com/techhounds/houndutil/houndlog/FaultLogger.java

@@ -11,10 +11,7 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StringArrayPublisher;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PowerDistribution;
-import edu.wpi.first.wpilibj.RobotBase;
-
 import java.lang.reflect.Field;
 import java.util.ArrayList;
 import java.util.HashSet;
@@ -266,6 +263,8 @@ public static void register(TalonFX talon) {
                 talon.getStickyFault_UnstableSupplyV(),
                 talon.getStickyFault_UsingFusedCANcoderWhileUnlicensed());
 
+        faultSignals.forEach((s) -> SignalManager.register(s));
+
         for (StatusSignal<Boolean> signal : faultSignals) {
             register(signal::getValue, "Talon FX [" + talon.getDeviceID() + "]", signal.getName(), FaultType.ERROR);
         }
@@ -294,6 +293,8 @@ public static void register(CANcoder cancoder) {
                 cancoder.getStickyFault_Undervoltage(),
                 cancoder.getStickyFault_UnlicensedFeatureInUse());
 
+        faultSignals.forEach((s) -> SignalManager.register(s));
+
         for (StatusSignal<Boolean> signal : faultSignals) {
             register(signal::getValue, "CANcoder [" + cancoder.getDeviceID() + "]", signal.getName(),
                     FaultType.ERROR);
@@ -334,6 +335,8 @@ public static void register(Pigeon2 pigeon) {
                 pigeon.getStickyFault_Undervoltage(),
                 pigeon.getStickyFault_UnlicensedFeatureInUse());
 
+        faultSignals.forEach((s) -> SignalManager.register(s));
+
         for (StatusSignal<Boolean> signal : faultSignals) {
             register(signal::getValue, "Pigeon 2 [" + pigeon.getDeviceID() + "]", signal.getName(),
                     FaultType.ERROR);