Maybe fixes

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

@@ -75,9 +75,9 @@ public record TalonFXFollower(Device.CAN id, MotorAlignmentValue opposesMain) {}
   // Preconfigured control objects reused for efficiency
   protected final CoastOut coastControl = new CoastOut();
   protected final StaticBrake brakeControl = new StaticBrake();
-  protected final VoltageOut voltageControl = new VoltageOut(0);
+  protected final VoltageOut voltageControl = new VoltageOut(0).withEnableFOC(true);
   protected final TorqueCurrentFOC currentControl = new TorqueCurrentFOC(0);
-  protected final DutyCycleOut dutyCycleControl = new DutyCycleOut(0);
+  protected final DutyCycleOut dutyCycleControl = new DutyCycleOut(0).withEnableFOC(true);
   protected final DynamicMotionMagicTorqueCurrentFOC positionControl =
       new DynamicMotionMagicTorqueCurrentFOC(0, 0, 0);
   protected final VelocityTorqueCurrentFOC velocityControl = new VelocityTorqueCurrentFOC(0);
@@ -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,15 +141,37 @@ 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(
+                50.0, position, velocity)); // Critical for control loops
     updateThread.CTRECheckErrorAndRetry(
         () ->
             BaseStatusSignal.setUpdateFrequencyForAll(
-                100, position, velocity, supplyCurrent, supplyCurrent, torqueCurrent, temperature));
+                20.0, supplyCurrent, torqueCurrent)); // Important for monitoring
 
+    // Non-critical telemetry (very low frequency)
     updateThread.CTRECheckErrorAndRetry(
         () ->
             BaseStatusSignal.setUpdateFrequencyForAll(
-                200, closedLoopError, closedLoopReference, closedLoopReferenceSlope));
+                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);
+    }
 
     motor.optimizeBusUtilization(0, 1.0);
   }
@@ -166,7 +189,7 @@ protected boolean isRunningPositionControl() {
     return (control instanceof PositionTorqueCurrentFOC)
         || (control instanceof PositionVoltage)
         || (control instanceof MotionMagicTorqueCurrentFOC)
-        || (control instanceof MotionMagicDutyCycle)
+        || (control instanceof DynamicMotionMagicTorqueCurrentFOC)
         || (control instanceof MotionMagicVoltage);
   }
 
@@ -181,7 +204,7 @@ protected boolean isRunningVelocityControl() {
     var control = motor.getAppliedControl();
     return (control instanceof VelocityTorqueCurrentFOC)
         || (control instanceof VelocityVoltage)
-        || (control instanceof MotionMagicVelocityDutyCycle)
+        || (control instanceof MotionMagicVelocityTorqueCurrentFOC)
         || (control instanceof MotionMagicVelocityVoltage);
   }
 
@@ -196,8 +219,8 @@ protected boolean isRunningVelocityControl() {
   protected boolean isRunningMotionMagic() {
     var control = motor.getAppliedControl();
     return (control instanceof MotionMagicTorqueCurrentFOC)
-        || (control instanceof MotionMagicDutyCycle)
-        || (control instanceof MotionMagicVelocityDutyCycle)
+        || (control instanceof DynamicMotionMagicTorqueCurrentFOC)
+        || (control instanceof MotionMagicVelocityTorqueCurrentFOC)
         || (control instanceof MotionMagicVoltage)
         || (control instanceof MotionMagicVelocityVoltage);
   }
@@ -241,54 +264,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();
@@ -395,7 +429,13 @@ public void runPosition(
       Velocity<AngularAccelerationUnit> maxJerk,
       PIDSlot slot) {
     this.goalPosition = position;
-    motor.setControl(positionControl.withPosition(position).withSlot(slot.getNum()));
+    motor.setControl(
+        positionControl
+            .withPosition(position)
+            .withVelocity(cruiseVelocity)
+            .withAcceleration(acceleration)
+            .withJerk(maxJerk)
+            .withSlot(slot.getNum()));
   }
 
   /**

src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismReal.java

@@ -107,4 +107,9 @@ public Angle getPosition() {
   public AngularVelocity getVelocity() {
     return inputs.velocity;
   }
+
+  @Override
+  public Current getSupplyCurrent() {
+    return inputs.supplyCurrent;
+  }
 }

src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java

@@ -172,28 +172,37 @@ public ModuleIOTalonFX(
     turnAppliedVolts = turnTalon.getMotorVoltage();
     turnCurrent = turnTalon.getStatorCurrent();
 
-    // Configure periodic frames
+    // Configure periodic frames with optimized frequencies
+    // Odometry signals need high frequency for accurate pose estimation
     BaseStatusSignal.setUpdateFrequencyForAll(
         Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition);
+    // Control signals need reduced frequency
+    BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, turnVelocity);
+
+    // Telemetry signals can use very low frequency
     BaseStatusSignal.setUpdateFrequencyForAll(
-        50.0,
-        driveVelocity,
-        driveAppliedVolts,
-        driveCurrent,
-        turnAbsolutePosition,
-        turnVelocity,
-        turnAppliedVolts,
-        turnCurrent);
+        10.0, driveAppliedVolts, driveCurrent, turnAppliedVolts, turnCurrent);
+
+    // Absolute encoder only needs very low updates since it's mainly used for initialization
+    BaseStatusSignal.setUpdateFrequencyForAll(20.0, turnAbsolutePosition);
+
+    // Optimize bus utilization for each motor with longer timeout for better optimization
     ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon);
+    cancoder.optimizeBusUtilization(1.0);
   }
 
   @Override
   public void updateInputs(ModuleIOInputs inputs) {
-    // Refresh all signals
-    var driveStatus =
-        BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
-    var turnStatus =
-        BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
+    // Refresh signals in priority groups
+    // Refresh critical odometry signals first
+    var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity);
+    var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity);
+
+    // Refresh telemetry signals
+    BaseStatusSignal.refreshAll(driveAppliedVolts, driveCurrent);
+    BaseStatusSignal.refreshAll(turnAppliedVolts, turnCurrent);
+
+    // Refresh absolute encoder with lower priority
     var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
 
     // Update drive inputs