Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Upgrading to 2025.1.0

Browse Source
This commit is contained in:
thenetworkgrinch
2024-12-09 23:26:04 +00:00
parent 9fe6551d88
commit 4bc6978a20
35 changed files with 1902 additions and 1122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

327
swervelib/motors/SparkFlexSwerve.java
View File

@@ -1,66 +1,90 @@
package swervelib.motors;
import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import com.revrobotics.CANSparkMax;
import com.revrobotics.MotorFeedbackSensor;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkAnalogSensor;
import com.revrobotics.SparkPIDController;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.telemetry.Alert;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
* An implementation of {@link CANSparkFlex} as a {@link SwerveMotor}.
* An implementation of {@link SparkFlex} as a {@link SwerveMotor}.
*/
public class SparkFlexSwerve extends SwerveMotor
{
/**
* {@link CANSparkFlex} Instance.
* {@link SparkFlex} Instance.
*/
private final CANSparkFlex motor;
private final SparkFlex motor;
/**
* Integrated encoder.
*/
public RelativeEncoder encoder;
public RelativeEncoder encoder;
/**
* Absolute encoder attached to the SparkMax (if exists)
*/
public SwerveAbsoluteEncoder absoluteEncoder;
public SwerveAbsoluteEncoder absoluteEncoder;
/**
* Closed-loop PID controller.
*/
public SparkPIDController pid;
public SparkClosedLoopController pid;
/**
* Supplier for the velocity of the motor controller.
*/
private Supplier<Double> velocity;
/**
* Supplier for the position of the motor controller.
*/
private Supplier<Double> position;
/**
* Factory default already occurred.
*/
private boolean factoryDefaultOccurred = false;
private boolean factoryDefaultOccurred = false;
/**
* An {@link Alert} for if there is an error configuring the motor.
*/
private Alert failureConfiguring;
private Alert failureConfiguring;
/**
* An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client.
*/
private Alert absoluteEncoderOffsetWarning;
private Alert absoluteEncoderOffsetWarning;
/**
* Configuration object for {@link SparkFlex} motor.
*/
private SparkFlexConfig cfg = new SparkFlexConfig();
/**
* Tracker for changes that need to be pushed.
*/
private boolean cfgUpdated = false;
/**
* Initialize the swerve motor.
*
* @param motor The SwerveMotor as a SparkFlex object.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param motorType {@link DCMotor} which the {@link SparkFlex} is attached to.
*/
public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor)
public SparkFlexSwerve(SparkFlex motor, boolean isDriveMotor, DCMotor motorType)
{
this.motor = motor;
this.isDriveMotor = isDriveMotor;
@@ -68,32 +92,34 @@ public class SparkFlexSwerve extends SwerveMotor
clearStickyFaults();
encoder = motor.getEncoder();
pid = motor.getPIDController();
pid.setFeedbackDevice(
encoder); // Configure feedback of the PID controller as the integrated encoder.
pid = motor.getClosedLoopController();
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder.
cfgUpdated = true;
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
failureConfiguring = new Alert("Motors",
"Failure configuring motor " +
motor.getDeviceId(),
Alert.AlertType.WARNING_TRACE);
AlertType.kWarning);
absoluteEncoderOffsetWarning = new Alert("Motors",
"IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " +
"absoluteEncoderOffset in the Swerve Module JSON!",
Alert.AlertType.WARNING);
AlertType.kWarning);
velocity = encoder::getVelocity;
position = encoder::getPosition;
}
/**
* Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
* Initialize the {@link SwerveMotor} as a {@link SparkFlex} connected to a Brushless Motor.
*
* @param id CAN ID of the SparkMax.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param motorType {@link DCMotor} which the {@link SparkFlex} is attached to.
*/
public SparkFlexSwerve(int id, boolean isDriveMotor)
public SparkFlexSwerve(int id, boolean isDriveMotor, DCMotor motorType)
{
this(new CANSparkFlex(id, MotorType.kBrushless), isDriveMotor);
this(new SparkFlex(id, MotorType.kBrushless), isDriveMotor, motorType);
}
/**
@@ -109,11 +135,33 @@ public class SparkFlexSwerve extends SwerveMotor
{
return;
}
Timer.delay(0.01);
Timer.delay(Units.Milliseconds.of(10).in(Seconds));
}
failureConfiguring.set(true);
}
/**
* Get the current configuration of the {@link SparkFlex}
*
* @return {@link SparkMaxConfig}
*/
public SparkFlexConfig getConfig()
{
return cfg;
}
/**
* Update the config for the {@link SparkFlex}
*
* @param cfgGiven Given {@link SparkFlexConfig} which should have minimal modifications.
*/
public void updateConfig(SparkFlexConfig cfgGiven)
{
cfg.apply(cfgGiven);
configureSparkFlex(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
cfgUpdated = false;
}
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -122,7 +170,8 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setVoltageCompensation(double nominalVoltage)
{
configureSparkFlex(() -> motor.enableVoltageCompensation(nominalVoltage));
cfg.voltageCompensation(nominalVoltage);
cfgUpdated = true;
}
/**
@@ -134,7 +183,9 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setCurrentLimit(int currentLimit)
{
configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit));
cfg.smartCurrentLimit(currentLimit);
cfgUpdated = true;
}
/**
@@ -145,8 +196,9 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setLoopRampRate(double rampRate)
{
configureSparkFlex(() -> motor.setOpenLoopRampRate(rampRate));
configureSparkFlex(() -> motor.setClosedLoopRampRate(rampRate));
cfg.closedLoopRampRate(rampRate)
.openLoopRampRate(rampRate);
cfgUpdated = true;
}
/**
@@ -160,6 +212,21 @@ public class SparkFlexSwerve extends SwerveMotor
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.getNeoVortex(1);
}
return simMotor;
}
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
@@ -177,11 +244,7 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void factoryDefaults()
{
if (!factoryDefaultOccurred)
{
configureSparkFlex(motor::restoreFactoryDefaults);
factoryDefaultOccurred = true;
}
// Do nothing
}
/**
@@ -205,12 +268,20 @@ public class SparkFlexSwerve extends SwerveMotor
if (encoder == null)
{
absoluteEncoder = null;
configureSparkFlex(() -> pid.setFeedbackDevice(this.encoder));
} else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor)
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfgUpdated = true;
velocity = this.encoder::getVelocity;
position = this.encoder::getPosition;
} else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
cfgUpdated = true;
absoluteEncoderOffsetWarning.set(true);
absoluteEncoder = encoder;
configureSparkFlex(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder()));
velocity = absoluteEncoder::getVelocity;
position = absoluteEncoder::getAbsolutePosition;
}
return this;
}
@@ -223,39 +294,70 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
cfg.signals
.absoluteEncoderPositionAlwaysOn(false)
.absoluteEncoderVelocityAlwaysOn(false)
.analogPositionAlwaysOn(false)
.analogVelocityAlwaysOn(false)
.analogVoltageAlwaysOn(false)
.externalOrAltEncoderPositionAlwaysOn(false)
.externalOrAltEncoderVelocityAlwaysOn(false)
.primaryEncoderPositionAlwaysOn(false)
.primaryEncoderVelocityAlwaysOn(false)
.iAccumulationAlwaysOn(false)
.appliedOutputPeriodMs(10)
.faultsPeriodMs(20)
;
if (absoluteEncoder == null)
{
configureSparkFlex(() -> encoder.setPositionConversionFactor(positionConversionFactor));
configureSparkFlex(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfg.encoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
// https://www.chiefdelphi.com/t/shooter-encoder/400211/11
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
cfg.encoder
.quadratureMeasurementPeriod(10)
.quadratureAverageDepth(2);
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
configureCANStatusFrames(10, 20, 20, 500, 500);
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
cfg.signals
.primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs(20);
} else
{
configureSparkFlex(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
positionConversionFactor);
} else
{
return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
positionConversionFactor);
}
});
configureSparkFlex(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
positionConversionFactor / 60);
} else
{
return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
positionConversionFactor / 60);
}
});
// By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
// This needs to be set to 20ms or under to properly update the swerve module position for odometry
// Configuration taken from 3005, the team who helped develop the Max Swerve:
// https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
// Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
// From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
// with limited testing 19ms did not return the same value while the module was constatntly rotating.
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
cfg.signals
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs(20);
cfg.absoluteEncoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
}
}
cfgUpdated = true;
}
/**
@@ -266,15 +368,10 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void configurePIDF(PIDFConfig config)
{
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
configureSparkFlex(() -> pid.setP(config.p, pidSlot));
configureSparkFlex(() -> pid.setI(config.i, pidSlot));
configureSparkFlex(() -> pid.setD(config.d, pidSlot));
configureSparkFlex(() -> pid.setFF(config.f, pidSlot));
configureSparkFlex(() -> pid.setIZone(config.iz, pidSlot));
configureSparkFlex(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot));
cfg.closedLoop.pidf(config.p, config.i, config.d, config.f)
.iZone(config.iz)
.outputRange(config.output.min, config.output.max);
cfgUpdated = true;
}
/**
@@ -286,30 +383,11 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
configureSparkFlex(() -> pid.setPositionPIDWrappingEnabled(true));
configureSparkFlex(() -> pid.setPositionPIDWrappingMinInput(minInput));
configureSparkFlex(() -> pid.setPositionPIDWrappingMaxInput(maxInput));
}
cfg.closedLoop
.positionWrappingEnabled(true)
.positionWrappingInputRange(minInput, maxInput);
cfgUpdated = true;
/**
* Set the CAN status frames.
*
* @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
* @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
* @param CANStatus2 Motor Position
* @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
* @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
*/
public void configureCANStatusFrames(
int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
{
configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
// TODO: Configure Status Frame 5 and 6 if necessary
// https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
/**
@@ -320,7 +398,8 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setMotorBrake(boolean isBrakeMode)
{
configureSparkFlex(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
cfgUpdated = true;
}
/**
@@ -331,10 +410,8 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setInverted(boolean inverted)
{
configureSparkFlex(() -> {
motor.setInverted(inverted);
return motor.getLastError();
});
cfg.inverted(inverted);
cfgUpdated = true;
}
/**
@@ -343,13 +420,8 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void burnFlash()
{
try
{
Thread.sleep(200);
} catch (Exception e)
{
}
configureSparkFlex(() -> motor.burnFlash());
motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
cfgUpdated = false;
}
/**
@@ -372,11 +444,14 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward)
{
boolean possibleBurnOutIssue = true;
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
if (cfgUpdated)
{
burnFlash();
Timer.delay(0.1); // Give 100ms to apply changes
}
if (isDriveMotor)
{
configureSparkFlex(() ->
@@ -454,7 +529,7 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public double getVelocity()
{
return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity();
return velocity.get();
}
/**
@@ -465,7 +540,7 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public double getPosition()
{
return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition();
return position.get();
}
/**
@@ -482,22 +557,4 @@ public class SparkFlexSwerve extends SwerveMotor
}
}
/**
* REV Slots for PID configuration.
*/
enum SparkMAX_slotIdx
{
/**
* Slot 1, used for position PID's.
*/
Position,
/**
* Slot 2, used for velocity PID's.
*/
Velocity,
/**
* Slot 3, used arbitrarily. (Documentation show simulations).
*/
Simulation
}
}