mirror of
https://github.com/BroncBotz3481/YAGSL
synced 2026-07-04 07:31:39 +00:00
Updated to 2024
This commit is contained in:
thenetworkgrinch
@@ -1,21 +1,18 @@
|
||||
package swervelib.motors;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.DemandType;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfigurator;
|
||||
import com.ctre.phoenix6.controls.MotionMagicVoltage;
|
||||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
import swervelib.encoders.SwerveAbsoluteEncoder;
|
||||
import swervelib.math.SwerveMath;
|
||||
import swervelib.parser.PIDFConfig;
|
||||
import swervelib.simulation.ctre.PhysicsSim;
|
||||
import swervelib.telemetry.SwerveDriveTelemetry;
|
||||
|
||||
/**
|
||||
* {@link com.ctre.phoenix.motorcontrol.can.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics.
|
||||
* {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics.
|
||||
*/
|
||||
public class TalonFXSwerve extends SwerveMotor
|
||||
{
|
||||
@@ -23,31 +20,31 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
/**
|
||||
* Factory default already occurred.
|
||||
*/
|
||||
private final boolean factoryDefaultOccurred = false;
|
||||
private final boolean factoryDefaultOccurred = false;
|
||||
/**
|
||||
* Current TalonFX configuration.
|
||||
*/
|
||||
private final TalonFXConfiguration configuration = new TalonFXConfiguration();
|
||||
private TalonFXConfiguration configuration = new TalonFXConfiguration();
|
||||
/**
|
||||
* Whether the absolute encoder is integrated.
|
||||
*/
|
||||
private final boolean absoluteEncoder = false;
|
||||
private final boolean absoluteEncoder = false;
|
||||
/**
|
||||
* Motion magic angle voltage setter.
|
||||
*/
|
||||
private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0);
|
||||
// /**
|
||||
// * Motion Magic exponential voltage setters.
|
||||
// */
|
||||
// private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0);
|
||||
/**
|
||||
* Velocity voltage setter for controlling drive motor.
|
||||
*/
|
||||
private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
|
||||
/**
|
||||
* TalonFX motor controller.
|
||||
*/
|
||||
WPI_TalonFX motor;
|
||||
/**
|
||||
* The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to Degrees.
|
||||
*/
|
||||
private double positionConversionFactor = 1;
|
||||
/**
|
||||
* If the TalonFX configuration has changed.
|
||||
*/
|
||||
private boolean configChanged = true;
|
||||
/**
|
||||
* Nominal voltage default to use with feedforward.
|
||||
*/
|
||||
private double nominalVoltage = 12.0;
|
||||
TalonFX motor;
|
||||
|
||||
/**
|
||||
* Constructor for TalonFX swerve motor.
|
||||
@@ -55,7 +52,7 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
* @param motor Motor to use.
|
||||
* @param isDriveMotor Whether this motor is a drive motor.
|
||||
*/
|
||||
public TalonFXSwerve(WPI_TalonFX motor, boolean isDriveMotor)
|
||||
public TalonFXSwerve(TalonFX motor, boolean isDriveMotor)
|
||||
{
|
||||
this.isDriveMotor = isDriveMotor;
|
||||
this.motor = motor;
|
||||
@@ -63,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);
|
||||
// }
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -78,7 +75,7 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
*/
|
||||
public TalonFXSwerve(int id, String canbus, boolean isDriveMotor)
|
||||
{
|
||||
this(new WPI_TalonFX(id, canbus), isDriveMotor);
|
||||
this(new TalonFX(id, canbus), isDriveMotor);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -89,7 +86,7 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
*/
|
||||
public TalonFXSwerve(int id, boolean isDriveMotor)
|
||||
{
|
||||
this(new WPI_TalonFX(id), isDriveMotor);
|
||||
this(new TalonFX(id), isDriveMotor);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -100,10 +97,18 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
{
|
||||
if (!factoryDefaultOccurred)
|
||||
{
|
||||
motor.configFactoryDefault();
|
||||
motor.setSensorPhase(true);
|
||||
motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
|
||||
motor.configNeutralDeadband(0.001);
|
||||
TalonFXConfigurator cfg = motor.getConfigurator();
|
||||
configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake;
|
||||
configuration.ClosedLoopGeneral.ContinuousWrap = true;
|
||||
cfg.apply(configuration);
|
||||
|
||||
m_angleVoltageSetter.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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -146,7 +151,22 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public void configureIntegratedEncoder(double positionConversionFactor)
|
||||
{
|
||||
this.positionConversionFactor = positionConversionFactor;
|
||||
TalonFXConfigurator cfg = motor.getConfigurator();
|
||||
cfg.refresh(configuration);
|
||||
|
||||
positionConversionFactor = 1 / positionConversionFactor;
|
||||
|
||||
configuration.MotionMagic = configuration.MotionMagic
|
||||
.withMotionMagicCruiseVelocity(100 / positionConversionFactor)
|
||||
.withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100)
|
||||
.withMotionMagicExpo_kV(0.12 * positionConversionFactor)
|
||||
.withMotionMagicExpo_kA(0.1);
|
||||
|
||||
configuration.Feedback = configuration.Feedback
|
||||
.withSensorToMechanismRatio(positionConversionFactor)
|
||||
.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor);
|
||||
|
||||
cfg.apply(configuration);
|
||||
// Taken from democat's library.
|
||||
// https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16
|
||||
configureCANStatusFrames(250);
|
||||
@@ -159,7 +179,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);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -182,17 +202,17 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
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
|
||||
@@ -206,13 +226,14 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public void configurePIDF(PIDFConfig config)
|
||||
{
|
||||
configuration.slot0.kP = config.p;
|
||||
configuration.slot0.kI = config.i;
|
||||
configuration.slot0.kD = config.d;
|
||||
configuration.slot0.kF = config.f;
|
||||
configuration.slot0.integralZone = config.iz;
|
||||
configuration.slot0.closedLoopPeakOutput = config.output.max;
|
||||
configChanged = true;
|
||||
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;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -224,7 +245,10 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public void configurePIDWrapping(double minInput, double maxInput)
|
||||
{
|
||||
// Do nothing
|
||||
TalonFXConfigurator cfg = motor.getConfigurator();
|
||||
cfg.refresh(configuration.ClosedLoopGeneral);
|
||||
configuration.ClosedLoopGeneral.ContinuousWrap = true;
|
||||
cfg.apply(configuration.ClosedLoopGeneral);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -235,7 +259,7 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public void setMotorBrake(boolean isBrakeMode)
|
||||
{
|
||||
motor.setNeutralMode(isBrakeMode ? NeutralMode.Brake : NeutralMode.Coast);
|
||||
motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -246,7 +270,7 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public void setInverted(boolean inverted)
|
||||
{
|
||||
Timer.delay(1);
|
||||
// Timer.delay(1);
|
||||
motor.setInverted(inverted);
|
||||
}
|
||||
|
||||
@@ -256,11 +280,7 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public void burnFlash()
|
||||
{
|
||||
if (configChanged)
|
||||
{
|
||||
motor.configAllSettings(configuration, 0);
|
||||
configChanged = false;
|
||||
}
|
||||
// Do nothing
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -274,20 +294,6 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
motor.set(percentOutput);
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the setpoint into native sensor units.
|
||||
*
|
||||
* @param setpoint Setpoint to mutate. In meters per second or degrees.
|
||||
* @param position Position in degrees, only used on angle motors.
|
||||
* @return Setpoint as native sensor units. Encoder ticks per 100ms, or Encoder tick.
|
||||
*/
|
||||
public double convertToNativeSensorUnits(double setpoint, double position)
|
||||
{
|
||||
setpoint =
|
||||
isDriveMotor ? setpoint * .1 : SwerveMath.placeInAppropriate0To360Scope(position, setpoint);
|
||||
return setpoint / positionConversionFactor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the closed loop PID controller reference point.
|
||||
*
|
||||
@@ -310,27 +316,17 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public void setReference(double setpoint, double feedforward, double position)
|
||||
{
|
||||
if (SwerveDriveTelemetry.isSimulation)
|
||||
{
|
||||
PhysicsSim.getInstance().run();
|
||||
}
|
||||
|
||||
burnFlash();
|
||||
// if (SwerveDriveTelemetry.isSimulation)
|
||||
// {
|
||||
// PhysicsSim.getInstance().run();
|
||||
// }
|
||||
|
||||
if (isDriveMotor)
|
||||
{
|
||||
motor.set(
|
||||
TalonFXControlMode.Velocity,
|
||||
convertToNativeSensorUnits(setpoint, position),
|
||||
DemandType.ArbitraryFeedForward,
|
||||
feedforward / nominalVoltage);
|
||||
motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint));
|
||||
} else
|
||||
{
|
||||
motor.set(
|
||||
TalonFXControlMode.Position,
|
||||
convertToNativeSensorUnits(setpoint, position),
|
||||
DemandType.ArbitraryFeedForward,
|
||||
feedforward);
|
||||
motor.setControl(m_angleVoltageSetter.withPosition(setpoint));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -342,7 +338,7 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public double getVelocity()
|
||||
{
|
||||
return (motor.getSelectedSensorVelocity() * 10) * positionConversionFactor;
|
||||
return motor.getVelocity().getValue();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -353,7 +349,7 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public double getPosition()
|
||||
{
|
||||
return motor.getSelectedSensorPosition() * positionConversionFactor;
|
||||
return motor.getPosition().getValue();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -367,7 +363,8 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
|
||||
{
|
||||
position = position < 0 ? (position % 360) + 360 : position;
|
||||
motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 0);
|
||||
TalonFXConfigurator cfg = motor.getConfigurator();
|
||||
cfg.setPosition(position);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -379,9 +376,7 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public void setVoltageCompensation(double nominalVoltage)
|
||||
{
|
||||
configuration.voltageCompSaturation = nominalVoltage;
|
||||
configChanged = true;
|
||||
this.nominalVoltage = nominalVoltage;
|
||||
// Do not implement
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -393,9 +388,9 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public void setCurrentLimit(int currentLimit)
|
||||
{
|
||||
configuration.supplyCurrLimit.currentLimit = currentLimit;
|
||||
configuration.supplyCurrLimit.enable = true;
|
||||
configChanged = true;
|
||||
TalonFXConfigurator cfg = motor.getConfigurator();
|
||||
cfg.refresh(configuration.CurrentLimits);
|
||||
cfg.apply(configuration.CurrentLimits.withStatorCurrentLimit(currentLimit).withStatorCurrentLimitEnable(true));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -406,9 +401,9 @@ public class TalonFXSwerve extends SwerveMotor
|
||||
@Override
|
||||
public void setLoopRampRate(double rampRate)
|
||||
{
|
||||
configuration.closedloopRamp = rampRate;
|
||||
configuration.openloopRamp = rampRate;
|
||||
configChanged = true;
|
||||
TalonFXConfigurator cfg = motor.getConfigurator();
|
||||
cfg.refresh(configuration.ClosedLoopRamps);
|
||||
cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
Reference in New Issue
Block a user