Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-07-01 07:21:39 +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

315
swervelib/motors/SparkMaxSwerve.java
View File

@@ -1,91 +1,112 @@
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.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.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
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.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Supplier;
import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
* An implementation of {@link CANSparkMax} as a {@link SwerveMotor}.
* An implementation of {@link com.revrobotics.spark.SparkMax} as a {@link SwerveMotor}.
*/
public class SparkMaxSwerve extends SwerveMotor
{
/**
* {@link CANSparkMax} Instance.
* {@link SparkMax} Instance.
*/
private final CANSparkMax motor;
private final SparkMax 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;
/**
* Factory default already occurred.
*/
private boolean factoryDefaultOccurred = false;
private boolean factoryDefaultOccurred = false;
/**
* Supplier for the velocity of the motor controller.
*/
private Supplier<Double> velocity;
private Supplier<Double> velocity;
/**
* Supplier for the position of the motor controller.
*/
private Supplier<Double> position;
private Supplier<Double> position;
/**
* Configuration object for {@link SparkMax} motor.
*/
private SparkMaxConfig cfg = new SparkMaxConfig();
/**
* Tracker for changes that need to be pushed.
*/
private boolean cfgUpdated = false;
/**
* Initialize the swerve motor.
*
* @param motor The SwerveMotor as a SparkMax object.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param motorType Motor type controlled by the {@link SparkMax} motor controller.
*/
public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor)
public SparkMaxSwerve(SparkMax motor, boolean isDriveMotor, DCMotor motorType)
{
this.motor = motor;
this.isDriveMotor = isDriveMotor;
this.simMotor = motorType;
factoryDefaults();
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;
velocity = encoder::getVelocity;
position = encoder::getPosition;
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
}
/**
* Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
* Initialize the {@link SwerveMotor} as a {@link SparkMax} connected to a Brushless Motor.
*
* @param id CAN ID of the SparkMax.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param motorType Motor type controlled by the {@link SparkMax} motor controller.
*/
public SparkMaxSwerve(int id, boolean isDriveMotor)
public SparkMaxSwerve(int id, boolean isDriveMotor, DCMotor motorType)
{
this(new CANSparkMax(id, MotorType.kBrushless), isDriveMotor);
this(new SparkMax(id, MotorType.kBrushless), isDriveMotor, motorType);
}
/**
@@ -101,11 +122,33 @@ public class SparkMaxSwerve extends SwerveMotor
{
return;
}
Timer.delay(0.01);
Timer.delay(Units.Milliseconds.of(10).in(Seconds));
}
DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
}
/**
* Get the current configuration of the {@link SparkMax}
*
* @return {@link SparkMaxConfig}
*/
public SparkMaxConfig getConfig()
{
return cfg;
}
/**
* Update the config for the {@link SparkMax}
*
* @param cfgGiven Given {@link SparkMaxConfig} which should have minimal modifications.
*/
public void updateConfig(SparkMaxConfig cfgGiven)
{
cfg.apply(cfgGiven);
configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
cfgUpdated = false;
}
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -114,7 +157,8 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setVoltageCompensation(double nominalVoltage)
{
configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage));
cfg.voltageCompensation(nominalVoltage);
cfgUpdated = true;
}
/**
@@ -126,7 +170,9 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setCurrentLimit(int currentLimit)
{
configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit));
cfg.smartCurrentLimit(currentLimit);
cfgUpdated = true;
}
/**
@@ -137,8 +183,10 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setLoopRampRate(double rampRate)
{
configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate));
configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate));
cfg.closedLoopRampRate(rampRate)
.openLoopRampRate(rampRate);
cfgUpdated = true;
}
/**
@@ -152,6 +200,21 @@ public class SparkMaxSwerve 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.getNEO(1);
}
return simMotor;
}
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
@@ -169,11 +232,7 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void factoryDefaults()
{
if (!factoryDefaultOccurred)
{
configureSparkMax(motor::restoreFactoryDefaults);
factoryDefaultOccurred = true;
}
// Do nothing
}
/**
@@ -197,17 +256,23 @@ public class SparkMaxSwerve extends SwerveMotor
if (encoder == null)
{
absoluteEncoder = null;
configureSparkMax(() -> pid.setFeedbackDevice(this.encoder));
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfgUpdated = true;
velocity = this.encoder::getVelocity;
position = this.encoder::getPosition;
} else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor)
burnFlash();
} else if (encoder instanceof SparkMaxAnalogEncoderSwerve || encoder instanceof SparkMaxEncoderSwerve)
{
cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve
? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder);
cfgUpdated = true;
DriverStation.reportWarning(
"IF possible configure the encoder offset in the REV Hardware Client instead of using the" +
" absoluteEncoderOffset in the Swerve Module JSON!",
false);
absoluteEncoder = encoder;
configureSparkMax(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder()));
velocity = absoluteEncoder::getVelocity;
position = absoluteEncoder::getAbsolutePosition;
}
@@ -222,10 +287,26 @@ public class SparkMaxSwerve 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)
{
configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
configureSparkMax(() -> 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
@@ -233,54 +314,55 @@ public class SparkMaxSwerve extends SwerveMotor
// 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
configureSparkMax(() -> encoder.setMeasurementPeriod(10));
configureSparkMax(() -> encoder.setAverageDepth(2));
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
// 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.
configureCANStatusFrames(10, 20, 20, 500, 500, 200, 200);
cfg.signals
.primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs(20);
} else
{
// 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.
// 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)
{
configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50);
}
// Need to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use
else if (absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor)
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
cfg.signals
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs(20);
cfg.absoluteEncoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
} else
{
configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200);
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
cfg.signals
.analogVoltageAlwaysOn(true)
.analogPositionAlwaysOn(true)
.analogVoltagePeriodMs(20)
.analogPositionPeriodMs(20);
cfg.analogSensor
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
}
configureSparkMax(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
positionConversionFactor);
} else
{
return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
positionConversionFactor);
}
});
configureSparkMax(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
positionConversionFactor / 60);
} else
{
return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
positionConversionFactor / 60);
}
});
}
cfgUpdated = true;
}
/**
@@ -291,15 +373,11 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void configurePIDF(PIDFConfig config)
{
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
configureSparkMax(() -> pid.setP(config.p));
configureSparkMax(() -> pid.setI(config.i));
configureSparkMax(() -> pid.setD(config.d));
configureSparkMax(() -> pid.setFF(config.f));
configureSparkMax(() -> pid.setIZone(config.iz));
configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max));
cfg.closedLoop.pidf(config.p, config.i, config.d, config.f)
.iZone(config.iz)
.outputRange(config.output.min, config.output.max);
cfgUpdated = true;
}
/**
@@ -311,33 +389,11 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true));
configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput));
configureSparkMax(() -> 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
* @param CANStatus5 Duty Cycle Absolute Encoder Position, Duty Cycle Absolute Encoder Absolute Angle
* @param CANStatus6 Duty Cycle Absolute Encoder Velocity, Duty Cycle Absolute Encoder Frequency
*/
public void configureCANStatusFrames(
int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus5, int CANStatus6)
{
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, CANStatus5));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus6, CANStatus6));
// https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
/**
@@ -348,7 +404,9 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setMotorBrake(boolean isBrakeMode)
{
configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
cfgUpdated = true;
}
/**
@@ -359,10 +417,8 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setInverted(boolean inverted)
{
configureSparkMax(() -> {
motor.setInverted(inverted);
return motor.getLastError();
});
cfg.inverted(inverted);
cfgUpdated = true;
}
/**
@@ -371,13 +427,8 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void burnFlash()
{
try
{
Thread.sleep(200);
} catch (Exception e)
{
}
configureSparkMax(() -> motor.burnFlash());
motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
cfgUpdated = false;
}
/**
@@ -400,11 +451,14 @@ public class SparkMaxSwerve 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)
{
configureSparkMax(() ->
@@ -509,23 +563,4 @@ public class SparkMaxSwerve extends SwerveMotor
configureSparkMax(() -> encoder.setPosition(position));
}
}
/**
* 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
}
}