From c47e9cfd38b347529f7567ee0e6c902e93280029 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Wed, 17 Jan 2024 22:24:31 -0600 Subject: [PATCH] updated to fix TalonFX's --- swervelib/motors/TalonFXSwerve.java | 121 ++++++++++++++++------------ 1 file changed, 71 insertions(+), 50 deletions(-) diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java index fab3a01..e6e6b99 100644 --- a/swervelib/motors/TalonFXSwerve.java +++ b/swervelib/motors/TalonFXSwerve.java @@ -33,10 +33,10 @@ public class TalonFXSwerve extends SwerveMotor * Velocity voltage setter for controlling drive motor. */ private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); -// /** -// * Motion Magic exponential voltage setters. -// */ -// private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); + /** + * Conversion factor for the motor. + */ + private double conversionFactor; /** * TalonFX motor controller. */ @@ -60,10 +60,10 @@ public class TalonFXSwerve extends SwerveMotor factoryDefaults(); clearStickyFaults(); -// if (SwerveDriveTelemetry.isSimulation) -// { -//// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); -// } + // if (SwerveDriveTelemetry.isSimulation) + // { + //// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); + // } } /** @@ -103,12 +103,12 @@ public class TalonFXSwerve extends SwerveMotor cfg.apply(configuration); m_angleVoltageSetter.UpdateFreqHz = 0; -// m_angleVoltageExpoSetter.UpdateFreqHz = 0; + // m_angleVoltageExpoSetter.UpdateFreqHz = 0; m_velocityVoltageSetter.UpdateFreqHz = 0; -// motor.configFactoryDefault(); -// motor.setSensorPhase(true); -// motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); -// motor.configNeutralDeadband(0.001); + // motor.configFactoryDefault(); + // motor.setSensorPhase(true); + // motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); + // motor.configNeutralDeadband(0.001); } } @@ -155,16 +155,20 @@ public class TalonFXSwerve extends SwerveMotor cfg.refresh(configuration); positionConversionFactor = 1 / positionConversionFactor; + if (!isDriveMotor) + { + positionConversionFactor *= 360; + } + conversionFactor = positionConversionFactor; - configuration.MotionMagic = configuration.MotionMagic - .withMotionMagicCruiseVelocity(100 / positionConversionFactor) - .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100) - .withMotionMagicExpo_kV(0.12 * positionConversionFactor) - .withMotionMagicExpo_kA(0.1); + configuration.MotionMagic = + configuration.MotionMagic.withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) + .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100) + .withMotionMagicExpo_kV(0.12 * positionConversionFactor) + .withMotionMagicExpo_kA(0.1); - configuration.Feedback = configuration.Feedback - .withSensorToMechanismRatio(positionConversionFactor) - .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor); + configuration.Feedback.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) + .withSensorToMechanismRatio(positionConversionFactor); cfg.apply(configuration); // Taken from democat's library. @@ -179,7 +183,7 @@ public class TalonFXSwerve extends SwerveMotor */ public void configureCANStatusFrames(int CANStatus1) { -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); } /** @@ -198,21 +202,31 @@ public class TalonFXSwerve extends SwerveMotor * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current Measurement */ - public void configureCANStatusFrames(int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus8, - int CANStatus10, int CANStatus12, int CANStatus13, int CANStatus14, - int CANStatus21, int CANStatusCurrent) + public void configureCANStatusFrames( + int CANStatus1, + int CANStatus2, + int CANStatus3, + int CANStatus4, + int CANStatus8, + int CANStatus10, + int CANStatus12, + int CANStatus13, + int CANStatus14, + int CANStatus21, + int CANStatusCurrent) { -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, CANStatusCurrent); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, + // CANStatusCurrent); // TODO: Configure Status Frame 2 thru 21 if necessary // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods @@ -226,14 +240,19 @@ public class TalonFXSwerve extends SwerveMotor @Override public void configurePIDF(PIDFConfig config) { + if(!isDriveMotor) + { + config.p *= 360; + config.i *= 360; + config.d *= 360; + } + TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.Slot0); - cfg.apply(configuration.Slot0.withKP(config.p) - .withKI(config.i) - .withKD(config.d) - .withKS(config.f)); -// configuration.slot0.integralZone = config.iz; -// configuration.slot0.closedLoopPeakOutput = config.output.max; + cfg.apply( + configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f)); + // configuration.slot0.integralZone = config.iz; + // configuration.slot0.closedLoopPeakOutput = config.output.max; } /** @@ -270,7 +289,7 @@ public class TalonFXSwerve extends SwerveMotor @Override public void setInverted(boolean inverted) { -// Timer.delay(1); + // Timer.delay(1); motor.setInverted(inverted); } @@ -316,17 +335,17 @@ public class TalonFXSwerve extends SwerveMotor @Override public void setReference(double setpoint, double feedforward, double position) { -// if (SwerveDriveTelemetry.isSimulation) -// { -// PhysicsSim.getInstance().run(); -// } + // if (SwerveDriveTelemetry.isSimulation) + // { + // PhysicsSim.getInstance().run(); + // } if (isDriveMotor) { motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward)); } else { - motor.setControl(m_angleVoltageSetter.withPosition(setpoint)); + motor.setControl(m_angleVoltageSetter.withPosition(setpoint / 360.0)); } } @@ -364,7 +383,7 @@ public class TalonFXSwerve extends SwerveMotor { position = position < 0 ? (position % 360) + 360 : position; TalonFXConfigurator cfg = motor.getConfigurator(); - cfg.setPosition(position); + cfg.setPosition(position / 360); } } @@ -390,7 +409,9 @@ public class TalonFXSwerve extends SwerveMotor { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.CurrentLimits); - cfg.apply(configuration.CurrentLimits.withStatorCurrentLimit(currentLimit).withStatorCurrentLimitEnable(true)); + cfg.apply( + configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) + .withStatorCurrentLimitEnable(true)); } /**