mirror of
https://github.com/BroncBotz3481/YAGSL
synced 2026-06-19 06:21:40 +00:00
467 lines
13 KiB
Java
467 lines
13 KiB
Java
package swervelib.motors;
|
|
|
|
import static edu.wpi.first.units.Units.Degrees;
|
|
import static edu.wpi.first.units.Units.Rotations;
|
|
import static edu.wpi.first.units.Units.Volts;
|
|
|
|
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.InvertedValue;
|
|
import com.ctre.phoenix6.signals.NeutralModeValue;
|
|
import edu.wpi.first.math.system.plant.DCMotor;
|
|
import swervelib.encoders.SwerveAbsoluteEncoder;
|
|
import swervelib.parser.PIDFConfig;
|
|
import swervelib.telemetry.SwerveDriveTelemetry;
|
|
|
|
/**
|
|
* {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics.
|
|
*/
|
|
public class TalonFXSwerve extends SwerveMotor
|
|
{
|
|
|
|
/**
|
|
* Wait time for status frames to show up.
|
|
*/
|
|
public static double STATUS_TIMEOUT_SECONDS = 0.02;
|
|
/**
|
|
* Factory default already occurred.
|
|
*/
|
|
private final boolean factoryDefaultOccurred = false;
|
|
/**
|
|
* Whether the absolute encoder is integrated.
|
|
*/
|
|
private final boolean absoluteEncoder = false;
|
|
/**
|
|
* Motion magic angle voltage setter.
|
|
*/
|
|
private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0);
|
|
/**
|
|
* Velocity voltage setter for controlling drive motor.
|
|
*/
|
|
private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
|
|
/**
|
|
* TalonFX motor controller.
|
|
*/
|
|
private final TalonFX motor;
|
|
/**
|
|
* Conversion factor for the motor.
|
|
*/
|
|
private double conversionFactor;
|
|
/**
|
|
* Current TalonFX configuration.
|
|
*/
|
|
private TalonFXConfiguration configuration = new TalonFXConfiguration();
|
|
/**
|
|
* Current TalonFX Configurator.
|
|
*/
|
|
private TalonFXConfigurator cfg;
|
|
|
|
|
|
/**
|
|
* Constructor for TalonFX swerve motor.
|
|
*
|
|
* @param motor Motor to use.
|
|
* @param isDriveMotor Whether this motor is a drive motor.
|
|
* @param motorType {@link DCMotor} which the {@link TalonFX} is attached to.
|
|
*/
|
|
public TalonFXSwerve(TalonFX motor, boolean isDriveMotor, DCMotor motorType)
|
|
{
|
|
this.isDriveMotor = isDriveMotor;
|
|
this.motor = motor;
|
|
this.cfg = motor.getConfigurator();
|
|
this.simMotor = motorType;
|
|
|
|
factoryDefaults();
|
|
clearStickyFaults();
|
|
|
|
// if (SwerveDriveTelemetry.isSimulation)
|
|
// {
|
|
//// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800);
|
|
// }
|
|
}
|
|
|
|
/**
|
|
* Construct the TalonFX swerve motor given the ID and CANBus.
|
|
*
|
|
* @param id ID of the TalonFX on the CANBus.
|
|
* @param canbus CANBus on which the TalonFX is on.
|
|
* @param isDriveMotor Whether the motor is a drive or steering motor.
|
|
* @param motorType {@link DCMotor} which the {@link TalonFX} is attached to.
|
|
*/
|
|
public TalonFXSwerve(int id, String canbus, boolean isDriveMotor, DCMotor motorType)
|
|
{
|
|
this(new TalonFX(id, canbus), isDriveMotor, motorType);
|
|
}
|
|
|
|
/**
|
|
* Construct the TalonFX swerve motor given the ID.
|
|
*
|
|
* @param id ID of the TalonFX on the canbus.
|
|
* @param isDriveMotor Whether the motor is a drive or steering motor.
|
|
* @param motorType {@link DCMotor} which the {@link TalonFX} is attached to.
|
|
*/
|
|
public TalonFXSwerve(int id, boolean isDriveMotor, DCMotor motorType)
|
|
{
|
|
this(new TalonFX(id), isDriveMotor, motorType);
|
|
}
|
|
|
|
/**
|
|
* Configure the factory defaults.
|
|
*/
|
|
@Override
|
|
public void factoryDefaults()
|
|
{
|
|
if (!factoryDefaultOccurred)
|
|
{
|
|
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);
|
|
}
|
|
}
|
|
|
|
@Override
|
|
public void close() {
|
|
motor.close();
|
|
}
|
|
|
|
|
|
/**
|
|
* Clear the sticky faults on the motor controller.
|
|
*/
|
|
@Override
|
|
public void clearStickyFaults()
|
|
{
|
|
motor.clearStickyFaults();
|
|
}
|
|
|
|
/**
|
|
* Set the absolute encoder to be a compatible absolute encoder.
|
|
*
|
|
* @param encoder The encoder to use.
|
|
*/
|
|
@Override
|
|
public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
|
|
{
|
|
// Do not support.
|
|
return this;
|
|
}
|
|
|
|
/**
|
|
* Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity.
|
|
*
|
|
* @param positionConversionFactor The conversion factor to apply for position.
|
|
* <p><br>
|
|
* Degrees: <br>
|
|
* <code>
|
|
* 360 / (angleGearRatio * encoderTicksPerRotation)
|
|
* </code><br>
|
|
* <p><br>
|
|
* Meters:<br>
|
|
* <code>
|
|
* (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation)
|
|
* </code>
|
|
*/
|
|
@Override
|
|
public void configureIntegratedEncoder(double positionConversionFactor)
|
|
{
|
|
cfg.refresh(configuration);
|
|
|
|
positionConversionFactor = 1 / positionConversionFactor;
|
|
if (!isDriveMotor)
|
|
{
|
|
positionConversionFactor *= 360;
|
|
}
|
|
conversionFactor = positionConversionFactor;
|
|
|
|
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.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor)
|
|
.withSensorToMechanismRatio(positionConversionFactor);
|
|
|
|
cfg.apply(configuration);
|
|
}
|
|
|
|
/**
|
|
* Configure the PIDF values for the closed loop controller. 0 is disabled or off.
|
|
*
|
|
* @param config Configuration class holding the PIDF values.
|
|
*/
|
|
@Override
|
|
public void configurePIDF(PIDFConfig config)
|
|
{
|
|
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Configure the PID wrapping for the position closed loop controller.
|
|
*
|
|
* @param minInput Minimum PID input.
|
|
* @param maxInput Maximum PID input.
|
|
*/
|
|
@Override
|
|
public void configurePIDWrapping(double minInput, double maxInput)
|
|
{
|
|
cfg.refresh(configuration.ClosedLoopGeneral);
|
|
configuration.ClosedLoopGeneral.ContinuousWrap = true;
|
|
cfg.apply(configuration.ClosedLoopGeneral);
|
|
}
|
|
|
|
/**
|
|
* Disable PID Wrapping on the motor.
|
|
*/
|
|
@Override
|
|
public void disablePIDWrapping()
|
|
{
|
|
cfg.refresh(configuration.ClosedLoopGeneral);
|
|
configuration.ClosedLoopGeneral.ContinuousWrap = false;
|
|
cfg.apply(configuration.ClosedLoopGeneral);
|
|
}
|
|
|
|
/**
|
|
* Set the idle mode.
|
|
*
|
|
* @param isBrakeMode Set the brake mode.
|
|
*/
|
|
@Override
|
|
public void setMotorBrake(boolean isBrakeMode)
|
|
{
|
|
motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast);
|
|
}
|
|
|
|
/**
|
|
* Set the motor to be inverted.
|
|
*
|
|
* @param inverted State of inversion.
|
|
*/
|
|
@Override
|
|
public void setInverted(boolean inverted)
|
|
{
|
|
// Timer.delay(1);
|
|
cfg.refresh(configuration.MotorOutput);
|
|
configuration.MotorOutput.withInverted(
|
|
inverted ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive);
|
|
cfg.apply(configuration.MotorOutput);
|
|
}
|
|
|
|
/**
|
|
* Save the configurations from flash to EEPROM.
|
|
*/
|
|
@Override
|
|
public void burnFlash()
|
|
{
|
|
// Do nothing
|
|
}
|
|
|
|
/**
|
|
* Set the percentage output.
|
|
*
|
|
* @param percentOutput percent out for the motor controller.
|
|
*/
|
|
@Override
|
|
public void set(double percentOutput)
|
|
{
|
|
motor.set(percentOutput);
|
|
}
|
|
|
|
/**
|
|
* Set the closed loop PID controller reference point.
|
|
*
|
|
* @param setpoint Setpoint in MPS or Angle in degrees.
|
|
* @param feedforward Feedforward in volt-meter-per-second or kV.
|
|
*/
|
|
@Override
|
|
public void setReference(double setpoint, double feedforward)
|
|
{
|
|
setReference(setpoint, feedforward, getPosition());
|
|
}
|
|
|
|
/**
|
|
* Set the closed loop PID controller reference point.
|
|
*
|
|
* @param setpoint Setpoint in meters per second or angle in degrees.
|
|
* @param feedforward Feedforward in volt-meter-per-second or kV.
|
|
* @param position Only used on the angle motor, the position of the motor in degrees.
|
|
*/
|
|
@Override
|
|
public void setReference(double setpoint, double feedforward, double position)
|
|
{
|
|
// if (SwerveDriveTelemetry.isSimulation)
|
|
// {
|
|
// PhysicsSim.getInstance().run();
|
|
// }
|
|
|
|
if (isDriveMotor)
|
|
{
|
|
motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward));
|
|
} else
|
|
{
|
|
motor.setControl(m_angleVoltageSetter.withPosition(setpoint / 360.0));
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Get the voltage output of the motor controller.
|
|
*
|
|
* @return Voltage output.
|
|
*/
|
|
@Override
|
|
public double getVoltage()
|
|
{
|
|
return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(Volts);
|
|
}
|
|
|
|
/**
|
|
* Set the voltage of the motor.
|
|
*
|
|
* @param voltage Voltage to set.
|
|
*/
|
|
@Override
|
|
public void setVoltage(double voltage)
|
|
{
|
|
motor.setVoltage(voltage);
|
|
}
|
|
|
|
/**
|
|
* Get the applied dutycycle output.
|
|
*
|
|
* @return Applied dutycycle output to the motor.
|
|
*/
|
|
@Override
|
|
public double getAppliedOutput()
|
|
{
|
|
return motor.getDutyCycle().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue();
|
|
}
|
|
|
|
/**
|
|
* Get the velocity of the integrated encoder.
|
|
*
|
|
* @return velocity in Meters Per Second, or Degrees per Second.
|
|
*/
|
|
@Override
|
|
public double getVelocity()
|
|
{
|
|
return motor.getVelocity().getValue().magnitude();
|
|
}
|
|
|
|
/**
|
|
* Get the position of the integrated encoder.
|
|
*
|
|
* @return Position in Meters or Degrees.
|
|
*/
|
|
@Override
|
|
public double getPosition()
|
|
{
|
|
return motor.getPosition().getValue().magnitude();
|
|
}
|
|
|
|
/**
|
|
* Set the integrated encoder position.
|
|
*
|
|
* @param position Integrated encoder position. Should be angle in degrees or meters.
|
|
*/
|
|
@Override
|
|
public void setPosition(double position)
|
|
{
|
|
if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
|
|
{
|
|
cfg.setPosition(Degrees.of(position).in(Rotations));
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Set the voltage compensation for the swerve module motor.
|
|
*
|
|
* @param nominalVoltage Nominal voltage for operation to output to.
|
|
*/
|
|
@Override
|
|
public void setVoltageCompensation(double nominalVoltage)
|
|
{
|
|
// Do not implement
|
|
}
|
|
|
|
/**
|
|
* Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
|
|
* voltage compensation. This is useful to protect the motor from current spikes.
|
|
*
|
|
* @param currentLimit Current limit in AMPS at free speed.
|
|
*/
|
|
@Override
|
|
public void setCurrentLimit(int currentLimit)
|
|
{
|
|
cfg.refresh(configuration.CurrentLimits);
|
|
cfg.apply(
|
|
configuration.CurrentLimits.withSupplyCurrentLimit(currentLimit)
|
|
.withSupplyCurrentLimitEnable(true));
|
|
}
|
|
|
|
/**
|
|
* Set the maximum rate the open/closed loop output can change by.
|
|
*
|
|
* @param rampRate Time in seconds to go from 0 to full throttle.
|
|
*/
|
|
@Override
|
|
public void setLoopRampRate(double rampRate)
|
|
{
|
|
cfg.refresh(configuration.ClosedLoopRamps);
|
|
cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate));
|
|
}
|
|
|
|
/**
|
|
* Get the motor object from the module.
|
|
*
|
|
* @return Motor object.
|
|
*/
|
|
@Override
|
|
public Object getMotor()
|
|
{
|
|
return motor;
|
|
}
|
|
|
|
/**
|
|
* Get the {@link DCMotor} of the motor class.
|
|
*
|
|
* @return {@link DCMotor} of this type.
|
|
*/
|
|
@Override
|
|
public DCMotor getSimMotor()
|
|
{
|
|
if (simMotor == null)
|
|
{
|
|
simMotor = DCMotor.getKrakenX60(1);
|
|
}
|
|
return simMotor;
|
|
}
|
|
|
|
/**
|
|
* Queries whether the absolute encoder is directly attached to the motor controller.
|
|
*
|
|
* @return connected absolute encoder state.
|
|
*/
|
|
@Override
|
|
public boolean usingExternalFeedbackSensor()
|
|
{
|
|
return absoluteEncoder;
|
|
}
|
|
}
|