diff --git a/src/main/java/frc/robot/constant/swerve/SwerveConstants.java b/src/main/java/frc/robot/constant/swerve/SwerveConstants.java index a4f1cef..10a41e2 100644 --- a/src/main/java/frc/robot/constant/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/constant/swerve/SwerveConstants.java @@ -28,6 +28,10 @@ public final class SwerveConstants { public static final double kShootingSpeedMultiplier = 1.0; + /** Raises TalonFX status frame rates and costs CAN bandwidth; keep off at competition. */ + public static final boolean kLogMotorTelemetry = false; + public static final double kMotorTelemetryHz = 50.0; + ////////////////////////////////////// public Translation2d rearLeftTranslation; diff --git a/src/main/java/frc/robot/hardware/WheelMoverTalonFX.java b/src/main/java/frc/robot/hardware/WheelMoverTalonFX.java index d43f8af..e6a53aa 100644 --- a/src/main/java/frc/robot/hardware/WheelMoverTalonFX.java +++ b/src/main/java/frc/robot/hardware/WheelMoverTalonFX.java @@ -2,6 +2,7 @@ import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -38,6 +39,15 @@ public class WheelMoverTalonFX extends WheelMoverBase { private CANcoder turnCANcoder; + private BaseStatusSignal driveVoltsSignal; + private BaseStatusSignal driveStatorSignal; + private BaseStatusSignal driveSupplySignal; + private BaseStatusSignal driveVelocitySignal; + private BaseStatusSignal turnVoltsSignal; + private BaseStatusSignal turnStatorSignal; + private BaseStatusSignal turnSupplySignal; + private BaseStatusSignal turnVelocitySignal; + public WheelMoverTalonFX( int driveMotorChannel, InvertedValue driveMotorReversed, @@ -123,6 +133,24 @@ public WheelMoverTalonFX( m_turnMotor.setPosition( turnCANcoder.getAbsolutePosition().getValueAsDouble()); + + if (SwerveConstants.kLogMotorTelemetry) { + driveVoltsSignal = m_driveMotor.getMotorVoltage(false); + driveStatorSignal = m_driveMotor.getStatorCurrent(false); + driveSupplySignal = m_driveMotor.getSupplyCurrent(false); + driveVelocitySignal = m_driveMotor.getVelocity(false); + turnVoltsSignal = m_turnMotor.getMotorVoltage(false); + turnStatorSignal = m_turnMotor.getStatorCurrent(false); + turnSupplySignal = m_turnMotor.getSupplyCurrent(false); + turnVelocitySignal = m_turnMotor.getVelocity(false); + + // Velocity is deliberately absent: it shares a frame with position, which odometry + // reads, and pinning it here would cap that frame at kMotorTelemetryHz. + BaseStatusSignal.setUpdateFrequencyForAll( + SwerveConstants.kMotorTelemetryHz, + driveVoltsSignal, driveStatorSignal, driveSupplySignal, + turnVoltsSignal, turnStatorSignal, turnSupplySignal); + } } @Override @@ -238,6 +266,27 @@ private void logEverything(LinearVelocity requestedMps, Angle requestedAngle) { Logger.recordOutput(base + "rawCurrentAngle", rawAngle); + if (SwerveConstants.kLogMotorTelemetry) { + BaseStatusSignal.refreshAll( + driveVoltsSignal, driveStatorSignal, driveSupplySignal, driveVelocitySignal, + turnVoltsSignal, turnStatorSignal, turnSupplySignal, turnVelocitySignal); + + Logger.recordOutput(base + "driveAppliedVolts", driveVoltsSignal.getValueAsDouble()); + Logger.recordOutput(base + "driveStatorAmps", driveStatorSignal.getValueAsDouble()); + Logger.recordOutput(base + "driveSupplyAmps", driveSupplySignal.getValueAsDouble()); + Logger.recordOutput(base + "driveVelocityRps", driveVelocitySignal.getValueAsDouble()); + Logger.recordOutput(base + "driveVoltsFrameTimestamp", driveVoltsSignal.getTimestamp().getTime()); + Logger.recordOutput(base + "driveAmpsFrameTimestamp", driveStatorSignal.getTimestamp().getTime()); + Logger.recordOutput(base + "driveVelocityFrameTimestamp", driveVelocitySignal.getTimestamp().getTime()); + + Logger.recordOutput(base + "turnAppliedVolts", turnVoltsSignal.getValueAsDouble()); + Logger.recordOutput(base + "turnStatorAmps", turnStatorSignal.getValueAsDouble()); + Logger.recordOutput(base + "turnSupplyAmps", turnSupplySignal.getValueAsDouble()); + Logger.recordOutput(base + "turnVelocityRps", turnVelocitySignal.getValueAsDouble()); + Logger.recordOutput(base + "turnVoltsFrameTimestamp", turnVoltsSignal.getTimestamp().getTime()); + Logger.recordOutput(base + "turnAmpsFrameTimestamp", turnStatorSignal.getTimestamp().getTime()); + Logger.recordOutput(base + "turnVelocityFrameTimestamp", turnVelocitySignal.getTimestamp().getTime()); + } } // ***********************************************************************************************