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..e915ac2 100644 --- a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java +++ b/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 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..2f00452 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 @@ -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,19 +264,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 +282,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 +429,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; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index bb6a6d6..4b4124f 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); + // 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