R2 150 update shooter to use 2 flywheels

src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java

@@ -41,7 +41,13 @@ public AbsoluteEncoderIOCANCoder(
 
     angle = CANCoder.getAbsolutePosition();
 
-    updateThread.CTRECheckErrorAndRetry(() -> angle.setUpdateFrequency(200));
+    // Further reduce update frequency for absolute encoder since it's mainly used for
+    // initialization
+    updateThread.CTRECheckErrorAndRetry(
+        () -> angle.setUpdateFrequency(10.0)); // Further reduced from 25.0
+
+    // Optimize bus utilization
+    CANCoder.optimizeBusUtilization(1.0);
   }
 
   @Override

src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java

@@ -51,6 +51,13 @@ public DistanceSensorIOCANRange(Device.CAN id, String name, CANrangeConfiguratio
 
     ambientSignal = CANRange.getAmbientSignal();
     distance = CANRange.getDistance();
+
+    // Set very low update frequencies for distance sensors (not critical for control)
+    updateThread.CTRECheckErrorAndRetry(
+        () -> BaseStatusSignal.setUpdateFrequencyForAll(10.0, ambientSignal, distance));
+
+    // Optimize bus utilization
+    CANRange.optimizeBusUtilization(1.0);
   }
 
   @Override

src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java

@@ -105,6 +105,7 @@ public MotorIOTalonFX(
     this.name = name;
 
     motor = new TalonFX(main.id(), main.bus());
+    motor.optimizeBusUtilization();
     updateThread.CTRECheckErrorAndRetry(() -> motor.getConfigurator().apply(config));
 
     // Initialize lists
@@ -140,17 +141,38 @@ public MotorIOTalonFX(
     closedLoopReference = motor.getClosedLoopReference();
     closedLoopReferenceSlope = motor.getClosedLoopReferenceSlope();
 
+    // Set different update frequencies based on signal importance
+    // Critical signals for control (reduced frequency)
     updateThread.CTRECheckErrorAndRetry(
         () ->
             BaseStatusSignal.setUpdateFrequencyForAll(
-                100, position, velocity, supplyCurrent, supplyCurrent, torqueCurrent, temperature));
+                50.0, position, velocity)); // Critical for control loops
 
+    // Important telemetry (lower frequency)
     updateThread.CTRECheckErrorAndRetry(
         () ->
             BaseStatusSignal.setUpdateFrequencyForAll(
-                200, closedLoopError, closedLoopReference, closedLoopReferenceSlope));
+                20.0, supplyCurrent, torqueCurrent)); // Important for monitoring
 
-    motor.optimizeBusUtilization(0, 1.0);
+    // Non-critical telemetry (very low frequency)
+    updateThread.CTRECheckErrorAndRetry(
+        () ->
+            BaseStatusSignal.setUpdateFrequencyForAll(
+                10.0, temperature, supplyVoltage)); // Less critical
+
+    // Control loop feedback (reduced frequency when in closed-loop)
+    updateThread.CTRECheckErrorAndRetry(
+        () ->
+            BaseStatusSignal.setUpdateFrequencyForAll(
+                100.0, closedLoopError, closedLoopReference, closedLoopReferenceSlope));
+
+    // Optimize bus utilization with longer timeout for better optimization
+    motor.optimizeBusUtilization(1.0, 1.0); // Increased timeout for better optimization
+
+    // Optimize follower bus utilization
+    for (TalonFX follower : followers) {
+      follower.optimizeBusUtilization(1.0, 1.0);
+    }
   }
 
   /**
@@ -241,54 +263,65 @@ protected ControlType getCurrentControlType() {
    */
   @Override
   public void updateInputs(MotorInputs inputs) {
-    inputs.connected =
-        BaseStatusSignal.refreshAll(
-                position,
-                velocity,
-                supplyVoltage,
-                supplyCurrent,
-                torqueCurrent,
-                temperature,
-                closedLoopError,
-                closedLoopReference,
-                closedLoopReferenceSlope)
+    // Refresh signals in groups based on priority
+    // Refresh critical signals first
+    boolean criticalSignalsOk = BaseStatusSignal.refreshAll(position, velocity).isOK();
+
+    // Refresh telemetry signals
+    boolean telemetrySignalsOk =
+        BaseStatusSignal.refreshAll(supplyCurrent, torqueCurrent, temperature, supplyVoltage)
             .isOK();
 
+    inputs.connected = criticalSignalsOk && telemetrySignalsOk;
+
     inputs.position = position.getValue();
     inputs.velocity = velocity.getValue();
     inputs.appliedVoltage = supplyVoltage.getValue();
     inputs.supplyCurrent = supplyCurrent.getValue();
     inputs.torqueCurrent = torqueCurrent.getValue();
     inputs.temperature = temperature.getValue();
 
-    // Interpret control-loop status signals conditionally based on current mode
-    Double closedLoopErrorValue = closedLoopError.getValue();
-    Double closedLoopTargetValue = closedLoopReference.getValue();
-
+    // Check current control modes
     boolean isRunningPositionControl = isRunningPositionControl();
     boolean isRunningMotionMagic = isRunningMotionMagic();
     boolean isRunningVelocityControl = isRunningVelocityControl();
 
-    inputs.positionError = isRunningPositionControl ? Rotations.of(closedLoopErrorValue) : null;
-
-    inputs.activeTrajectoryPosition =
-        isRunningPositionControl && isRunningMotionMagic
-            ? Rotations.of(closedLoopTargetValue)
-            : null;
-
-    inputs.goalPosition = isRunningPositionControl ? goalPosition : null;
-
-    if (isRunningVelocityControl) {
-      inputs.velocityError = RotationsPerSecond.of(closedLoopErrorValue);
-      inputs.activeTrajectoryVelocity = RotationsPerSecond.of(closedLoopTargetValue);
-    } else if (isRunningPositionControl && isRunningMotionMagic) {
-      var targetVelocity = closedLoopReferenceSlope.getValue();
-      inputs.velocityError =
-          RotationsPerSecond.of(targetVelocity - inputs.velocity.in(RotationsPerSecond));
-      inputs.activeTrajectoryVelocity = RotationsPerSecond.of(targetVelocity);
+    // Only update closed-loop signals when in closed-loop modes
+    if (isRunningPositionControl || isRunningVelocityControl) {
+      BaseStatusSignal.refreshAll(closedLoopError, closedLoopReference, closedLoopReferenceSlope);
+
+      // Interpret control-loop status signals conditionally based on current mode
+      Double closedLoopErrorValue = closedLoopError.getValue();
+      Double closedLoopTargetValue = closedLoopReference.getValue();
+
+      inputs.positionError = isRunningPositionControl ? Rotations.of(closedLoopErrorValue) : null;
+
+      inputs.activeTrajectoryPosition =
+          isRunningPositionControl && isRunningMotionMagic
+              ? Rotations.of(closedLoopTargetValue)
+              : null;
+
+      inputs.goalPosition = isRunningPositionControl ? goalPosition : null;
+
+      if (isRunningVelocityControl) {
+        inputs.velocityError = RotationsPerSecond.of(closedLoopErrorValue);
+        inputs.activeTrajectoryVelocity = RotationsPerSecond.of(closedLoopTargetValue);
+      } else if (isRunningPositionControl && isRunningMotionMagic) {
+        var targetVelocity = closedLoopReferenceSlope.getValue();
+        inputs.velocityError =
+            RotationsPerSecond.of(targetVelocity - inputs.velocity.in(RotationsPerSecond));
+        inputs.activeTrajectoryVelocity = RotationsPerSecond.of(targetVelocity);
+      } else {
+        inputs.velocityError = null;
+        inputs.activeTrajectoryVelocity = null;
+      }
     } else {
+      // Not in closed-loop mode, set control loop values to null
+      inputs.positionError = null;
       inputs.velocityError = null;
       inputs.activeTrajectoryVelocity = null;
+      inputs.activeTrajectoryPosition = null;
+      inputs.goalPosition = null;
     }
 
     inputs.controlType = getCurrentControlType();