From d4aa5511cfd4c5bcd4ffcc292f8d6e7832435227 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Thu, 19 Mar 2026 17:29:43 -0400 Subject: [PATCH 1/5] oops --- .../AbsoluteEncoderIOCANCoder.java | 7 + .../DistanceSensorIOCANRange.java | 7 + .../frc/lib/W8/io/motor/MotorIOTalonFX.java | 121 ++++++++++++------ .../rotary/RotaryMechanismReal.java | 5 + 4 files changed, 98 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java index d6a3641..a817360 100644 --- a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java +++ b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java @@ -41,6 +41,13 @@ public AbsoluteEncoderIOCANCoder( angle = CANCoder.getAbsolutePosition(); + // 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); updateThread.CTRECheckErrorAndRetry(() -> angle.setUpdateFrequency(200)); } diff --git a/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java b/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java index 90c66a2..02a2d54 100644 --- a/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java +++ b/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 diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java index 2bbd047..bedfacc 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java +++ b/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 @@ -143,14 +144,33 @@ public MotorIOTalonFX( updateThread.CTRECheckErrorAndRetry( () -> BaseStatusSignal.setUpdateFrequencyForAll( - 100, position, velocity, supplyCurrent, supplyCurrent, torqueCurrent, temperature)); - + 50.0, position, velocity)); // Critical for control loops 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); + } } /** @@ -166,7 +186,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 +201,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 +216,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,19 +261,17 @@ 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(); @@ -261,34 +279,47 @@ public void updateInputs(MotorInputs inputs) { 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 +426,13 @@ public void runPosition( Velocity 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())); } /** diff --git a/src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismReal.java b/src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismReal.java index b8b7b62..d83d8df 100644 --- a/src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismReal.java +++ b/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; + } } From 171fa83c720fb3f13273b945ff9ccd9d307fa9a7 Mon Sep 17 00:00:00 2001 From: rhit-collinkn <95197995+rhit-collinkn@users.noreply.github.com> Date: Thu, 19 Mar 2026 17:35:30 -0400 Subject: [PATCH 2/5] 1 more Reduced update frequency for angle to optimize performance. --- .../frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java index a817360..e915ac2 100644 --- a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java +++ b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java @@ -48,7 +48,6 @@ public AbsoluteEncoderIOCANCoder( // Optimize bus utilization CANCoder.optimizeBusUtilization(1.0); - updateThread.CTRECheckErrorAndRetry(() -> angle.setUpdateFrequency(200)); } @Override From 9b56510529a3f9ba8c0d23a21702439722a8f4f9 Mon Sep 17 00:00:00 2001 From: rhit-collinkn <95197995+rhit-collinkn@users.noreply.github.com> Date: Thu, 19 Mar 2026 17:40:53 -0400 Subject: [PATCH 3/5] 1 more again Set different update frequencies for critical and non-critical signals. Optimize bus utilization for the motor and followers. --- src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java index bedfacc..2f00452 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java +++ b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java @@ -141,6 +141,9 @@ 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( @@ -150,8 +153,6 @@ public MotorIOTalonFX( BaseStatusSignal.setUpdateFrequencyForAll( 20.0, supplyCurrent, torqueCurrent)); // Important for monitoring - motor.optimizeBusUtilization(0, 1.0); - // Non-critical telemetry (very low frequency) updateThread.CTRECheckErrorAndRetry( () -> @@ -171,6 +172,8 @@ public MotorIOTalonFX( for (TalonFX follower : followers) { follower.optimizeBusUtilization(1.0, 1.0); } + + motor.optimizeBusUtilization(0, 1.0); } /** From a266477cf5e7fad00beb5e621b12a3fdd59b4209 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Thu, 19 Mar 2026 17:45:17 -0400 Subject: [PATCH 4/5] oops --- .../subsystems/drive/ModuleIOTalonFX.java | 39 ++++++++++++------- 1 file changed, 24 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index bb6a6d6..48b4a8e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/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); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); + // Control signals need reduced frequency + BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, turnVelocity); + + // Telemetry signals can use very low frequency + BaseStatusSignal.setUpdateFrequencyForAll( + 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 From 48a817ceb57bf7a882c03bf69e9c2f09c50882ce Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Thu, 19 Mar 2026 17:47:10 -0400 Subject: [PATCH 5/5] spotless --- .../java/frc/robot/subsystems/drive/ModuleIOTalonFX.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 48b4a8e..4b4124f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -180,8 +180,8 @@ public ModuleIOTalonFX( BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, turnVelocity); // Telemetry signals can use very low frequency - BaseStatusSignal.setUpdateFrequencyForAll( - 10.0, driveAppliedVolts, driveCurrent, turnAppliedVolts, turnCurrent); + BaseStatusSignal.setUpdateFrequencyForAll( + 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); @@ -193,7 +193,7 @@ public ModuleIOTalonFX( @Override public void updateInputs(ModuleIOInputs inputs) { - // Refresh signals in priority groups + // Refresh signals in priority groups // Refresh critical odometry signals first var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity); var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity);