From 7ad11306c065c0260784c4e775eda7e8cc72c6f6 Mon Sep 17 00:00:00 2001 From: Kevin Barnard Date: Wed, 5 Feb 2025 16:50:53 -0800 Subject: [PATCH] Unify Spark motor & encoder classes --- src/main/java/swervelib/SwerveModule.java | 4 +- ...rve.java => SparkAnalogEncoderSwerve.java} | 106 ++-- ...derSwerve.java => SparkEncoderSwerve.java} | 131 ++--- .../encoders/SparkFlexEncoderSwerve.java | 151 ----- .../swervelib/motors/SparkFlexSwerve.java | 540 +----------------- .../motors/SparkMaxBrushedMotorSwerve.java | 11 +- .../java/swervelib/motors/SparkMaxSwerve.java | 515 +---------------- .../java/swervelib/motors/SparkSwerve.java | 507 ++++++++++++++++ .../swervelib/parser/json/DeviceJson.java | 20 +- .../swervelib/parser/json/ModuleJson.java | 4 +- 10 files changed, 650 insertions(+), 1339 deletions(-) rename src/main/java/swervelib/encoders/{SparkMaxAnalogEncoderSwerve.java => SparkAnalogEncoderSwerve.java} (60%) rename src/main/java/swervelib/encoders/{SparkMaxEncoderSwerve.java => SparkEncoderSwerve.java} (58%) delete mode 100644 src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java create mode 100644 src/main/java/swervelib/motors/SparkSwerve.java diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index bb0443407..d08ce7641 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -17,7 +17,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import swervelib.encoders.SparkMaxEncoderSwerve; +import swervelib.encoders.SparkEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.math.SwerveMath; import swervelib.motors.SparkMaxBrushedMotorSwerve; @@ -856,7 +856,7 @@ public void pushOffsetsToEncoders() // If the absolute encoder is attached. if (angleMotor instanceof SparkMaxSwerve || angleMotor instanceof SparkMaxBrushedMotorSwerve) { - if (absoluteEncoder instanceof SparkMaxEncoderSwerve) + if (absoluteEncoder instanceof SparkEncoderSwerve) { angleMotor.setAbsoluteEncoder(absoluteEncoder); if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkAnalogEncoderSwerve.java similarity index 60% rename from src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java rename to src/main/java/swervelib/encoders/SparkAnalogEncoderSwerve.java index bda63039f..80614f63f 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkAnalogEncoderSwerve.java @@ -1,67 +1,70 @@ package swervelib.encoders; -import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkAnalogSensor; -import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; -import java.util.function.Supplier; import swervelib.motors.SparkMaxBrushedMotorSwerve; import swervelib.motors.SparkMaxSwerve; +import swervelib.motors.SparkSwerve; import swervelib.motors.SwerveMotor; /** - * SparkMax absolute encoder, attached through the data port analog pin. + * SparkBase absolute encoder, attached through the data port analog pin. */ -public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder +public class SparkAnalogEncoderSwerve extends SwerveAbsoluteEncoder { /** - * {@link swervelib.motors.SparkMaxSwerve} or {@link swervelib.motors.SparkMaxBrushedMotorSwerve} object. + * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkBase analog port. */ - private final SwerveMotor sparkMax; + public SparkAnalogSensor encoder; + /** - * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port. + * {@link swervelib.motors.SparkMaxSwerve} or {@link swervelib.motors.SparkMaxBrushedMotorSwerve} object. */ - public SparkAnalogSensor encoder; + protected final SwerveMotor motor; + /** * An {@link Alert} for if there is a failure configuring the encoder. */ - private Alert failureConfiguring; + protected final Alert failureConfiguring; + /** * An {@link Alert} for if the absolute encoder does not support integrated offsets. */ - private Alert doesNotSupportIntegratedOffsets; + protected final Alert doesNotSupportIntegratedOffsets; /** - * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link SparkMax} motor data port - * analog pin. + * Create the {@link SparkAnalogEncoderSwerve} object as a analog sensor from the {@link SparkBase} + * motor data port analog pin. * - * @param motor Motor to create the encoder from. + * @param motor Motor to create the encoder from. * @param maxVoltage Maximum voltage for analog input reading. */ - public SparkMaxAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage) + public SparkAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage) { - if (motor.getMotor() instanceof SparkMax) + if (motor.getMotor() instanceof SparkBase) { - sparkMax = motor; - encoder = ((SparkMax) motor.getMotor()).getAnalog(); + this.motor = motor; + encoder = ((SparkBase) motor.getMotor()).getAnalog(); + motor.setAbsoluteEncoder(this); setConversionFactor(360.0 / maxVoltage); } else { - throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); + throw new RuntimeException("Motor given to instantiate SparkAnalogEncoderSwerve is not a SparkBase"); } failureConfiguring = new Alert( - "Encoders", - "Failure configuring SparkMax Analog Encoder", + "Encoders", + "Failure configuring SparkBase Analog Encoder", AlertType.kWarning); doesNotSupportIntegratedOffsets = new Alert( "Encoders", - "SparkMax Analog Sensors do not support integrated offsets", + "SparkBase Analog Sensors do not support integrated offsets", AlertType.kWarning); - } @Override @@ -73,23 +76,6 @@ public void close() // sparkMax.close(); } - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkMax(Supplier config) - { - for (int i = 0; i < maximumRetries; i++) - { - if (config.get() == REVLibError.kOk) - { - return; - } - } - failureConfiguring.set(true); - } - /** * Set the conversion factor of the {@link SparkMaxAnalogEncoderSwerve}. * @@ -97,14 +83,13 @@ private void configureSparkMax(Supplier config) */ public void setConversionFactor(double conversionFactor) { - SparkMaxConfig cfg = null; - if (sparkMax instanceof SparkMaxSwerve) + SparkBaseConfig cfg = null; + if (motor instanceof SparkMaxSwerve) { - cfg = ((SparkMaxSwerve) sparkMax).getConfig(); - - } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) + cfg = ((SparkMaxSwerve) motor).getConfig(); + } else if (motor instanceof SparkMaxBrushedMotorSwerve) { - cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); + cfg = ((SparkMaxBrushedMotorSwerve) motor).getConfig(); } if (cfg != null) { @@ -122,17 +107,15 @@ public void setConversionFactor(double conversionFactor) .positionConversionFactor(conversionFactor) .velocityConversionFactor(conversionFactor / 60); } - if (sparkMax instanceof SparkMaxSwerve) + if (motor instanceof SparkMaxSwerve) { - ((SparkMaxSwerve) sparkMax).updateConfig(cfg); - } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) + ((SparkMaxSwerve) motor).updateConfig(cfg); + } else if (motor instanceof SparkMaxBrushedMotorSwerve) { - ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); + ((SparkMaxBrushedMotorSwerve) motor).updateConfig((SparkMaxConfig) cfg); } - } - /** * Reset the encoder to factory defaults. */ @@ -143,8 +126,7 @@ public void factoryDefault() } /** - * Clear sticky faults on the encoder. - */ + * Clear sticky faults on the encoder. */ @Override public void clearStickyFaults() { @@ -159,16 +141,16 @@ public void clearStickyFaults() @Override public void configure(boolean inverted) { - if (sparkMax instanceof SparkMaxSwerve) - { - SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); + if (motor instanceof SparkSwerve) { + var sparkBase = (SparkSwerve) motor; + SparkBaseConfig cfg = sparkBase.getConfig(); cfg.analogSensor.inverted(inverted); - ((SparkMaxSwerve) sparkMax).updateConfig(cfg); - } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) - { - SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); + sparkBase.updateConfig(cfg); + } else if (motor instanceof SparkMaxBrushedMotorSwerve) { + var sparkMax = (SparkMaxBrushedMotorSwerve) motor; + SparkMaxConfig cfg = sparkMax.getConfig(); cfg.analogSensor.inverted(inverted); - ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); + sparkMax.updateConfig(cfg); } } diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkEncoderSwerve.java similarity index 58% rename from src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java rename to src/main/java/swervelib/encoders/SparkEncoderSwerve.java index d530a7365..e702139ff 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkEncoderSwerve.java @@ -1,93 +1,81 @@ package swervelib.encoders; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkAbsoluteEncoder; -import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; -import java.util.function.Supplier; +import swervelib.motors.SparkFlexSwerve; import swervelib.motors.SparkMaxBrushedMotorSwerve; import swervelib.motors.SparkMaxSwerve; +import swervelib.motors.SparkSwerve; import swervelib.motors.SwerveMotor; /** - * SparkMax absolute encoder, attached through the data port. + * SparkBase absolute encoder, attached through the data port. */ -public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder +public class SparkEncoderSwerve extends SwerveAbsoluteEncoder { /** - * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. + * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkBase. */ - public SparkAbsoluteEncoder encoder; + public final SparkAbsoluteEncoder encoder; + /** - * An {@link Alert} for if there is a failure configuring the encoder. + * {@link SparkMaxBrushedMotorSwerve} or {@link SparkSwerve} instance. */ - private Alert failureConfiguring; + protected final SwerveMotor motor; + /** - * An {@link Alert} for if there is a failure configuring the encoder offset. + * An {@link Alert} for if there is a failure configuring the encoder. */ - private Alert offsetFailure; + protected final Alert failureConfiguring; + /** - * {@link SparkMaxBrushedMotorSwerve} or {@link SparkMaxSwerve} instance. + * An {@link Alert} for if there is a failure configuring the encoder offset. */ - private SwerveMotor sparkMax; + protected final Alert offsetFailure; /** - * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link com.revrobotics.spark.SparkMax} - * motor. + * Create the {@link SparkEncoderSwerve} object as a duty cycle from the {@link + * com.revrobotics.spark.SparkBase} motor. * - * @param motor Motor to create the encoder from. + * @param motor Motor to create the encoder from. * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. */ - public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) + public SparkEncoderSwerve(SwerveMotor motor, int conversionFactor) { failureConfiguring = new Alert( - "Encoders", - "Failure configuring SparkMax Absolute Encoder", - AlertType.kWarning); + "Encoders", + "Failure configuring SparkBase Absolute Encoder", + AlertType.kWarning); offsetFailure = new Alert( - "Encoders", - "Failure to set Absolute Encoder Offset", - AlertType.kWarning); - if (motor.getMotor() instanceof SparkMax) + "Encoders", + "Failure to set Absolute Encoder Offset", + AlertType.kWarning); + if (motor.getMotor() instanceof SparkBase) { - sparkMax = motor; - encoder = ((SparkMax) motor.getMotor()).getAbsoluteEncoder(); + this.motor = motor; + encoder = ((SparkBase) motor.getMotor()).getAbsoluteEncoder(); + motor.setAbsoluteEncoder(this); setConversionFactor(conversionFactor); } else { - throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); + throw new RuntimeException("Motor given to instantiate SparkEncoderSwerve is not a SparkBase"); } } @Override public void close() { - // SPARK MAX encoder gets closed with the motor - // I don't think an encoder getting closed should - // close the entire motor so i will keep this empty - // sparkFlex.close(); - } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkMax(Supplier config) - { - for (int i = 0; i < maximumRetries; i++) - { - if (config.get() == REVLibError.kOk) - { - return; - } - } - failureConfiguring.set(true); + // SPARK MAX/FLEX encoder gets closed with the motor + // I don't think an encoder getting closed should + // close the entire motor so i will keep this empty + // sparkFlex.close(); } /** @@ -116,22 +104,17 @@ public void clearStickyFaults() @Override public void configure(boolean inverted) { - if (sparkMax instanceof SparkMaxSwerve) + if (motor instanceof SparkSwerve) { - SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); + var sparkBase = (SparkSwerve) motor; + SparkBaseConfig cfg = sparkBase.getConfig(); cfg.absoluteEncoder.inverted(inverted); - ((SparkMaxSwerve) sparkMax).updateConfig(cfg); - } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) - { - SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); - cfg.absoluteEncoder.inverted(inverted); - ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); + sparkBase.updateConfig(cfg); } } - /** - * Set the conversion factor of the {@link SparkMaxEncoderSwerve}. + * Set the conversion factor of the {@link SparkEncoderSwerve}. * * @param conversionFactor Position conversion factor from ticks to unit. */ @@ -145,14 +128,13 @@ public void setConversionFactor(double conversionFactor) // 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. - SparkMaxConfig cfg = null; - if (sparkMax instanceof SparkMaxSwerve) + SparkBaseConfig cfg = null; + if (motor instanceof SparkSwerve) { - cfg = ((SparkMaxSwerve) sparkMax).getConfig(); - - } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) + cfg = ((SparkSwerve) motor).getConfig(); + } else if (motor instanceof SparkMaxBrushedMotorSwerve) { - cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); + cfg = ((SparkMaxBrushedMotorSwerve) motor).getConfig(); } if (cfg != null) { @@ -166,17 +148,15 @@ public void setConversionFactor(double conversionFactor) .positionConversionFactor(conversionFactor) .velocityConversionFactor(conversionFactor / 60); } - if (sparkMax instanceof SparkMaxSwerve) + if (motor instanceof SparkSwerve) { - ((SparkMaxSwerve) sparkMax).updateConfig(cfg); - } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) + ((SparkMaxSwerve) motor).updateConfig(cfg); + } else if (motor instanceof SparkMaxBrushedMotorSwerve) { - ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); + ((SparkMaxBrushedMotorSwerve) motor).updateConfig((SparkMaxConfig) cfg); } - } - /** * Get the absolute position of the encoder. * @@ -200,7 +180,7 @@ public Object getAbsoluteEncoder() } /** - * Sets the Absolute Encoder Offset inside of the SparkMax's Memory. + * Sets the Absolute Encoder Offset inside of the SparkBase's Memory. * * @param offset the offset the Absolute Encoder uses as the zero point. * @return if setting Absolute Encoder Offset was successful or not. @@ -208,6 +188,14 @@ public Object getAbsoluteEncoder() @Override public boolean setAbsoluteEncoderOffset(double offset) { + if (motor instanceof SparkFlexSwerve) + { + var sparkFlex = (SparkFlexSwerve) motor; + SparkBaseConfig cfg = sparkFlex.getConfig(); + cfg.absoluteEncoder.zeroOffset(offset); + sparkFlex.updateConfig(cfg); + return true; + } return false; } @@ -221,4 +209,5 @@ public double getVelocity() { return encoder.getVelocity(); } + } diff --git a/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java deleted file mode 100644 index 52deb9968..000000000 --- a/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java +++ /dev/null @@ -1,151 +0,0 @@ -package swervelib.encoders; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.REVLibError; -import com.revrobotics.spark.SparkAbsoluteEncoder; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import java.util.function.Supplier; -import swervelib.motors.SparkFlexSwerve; -import swervelib.motors.SwerveMotor; - -/** - * SparkFlex absolute encoder, attached through the data port. - */ -public class SparkFlexEncoderSwerve extends SwerveAbsoluteEncoder -{ - - /** - * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkFlex. - */ - public SparkAbsoluteEncoder encoder; - /** - * An {@link Alert} for if there is a failure configuring the encoder. - */ - private Alert failureConfiguring; - /** - * {@link SparkFlexSwerve} instance. - */ - private SwerveMotor sparkFlex; - - /** - * Create the {@link SparkFlexEncoderSwerve} object as a duty cycle from the {@link SparkFlex} motor. - * - * @param motor Motor to create the encoder from. - * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. - */ - public SparkFlexEncoderSwerve(SwerveMotor motor, int conversionFactor) - { - failureConfiguring = new Alert( - "Encoders", - "Failure configuring SparkFlex Absolute Encoder", - AlertType.kWarning); - if (motor.getMotor() instanceof SparkFlex) - { - sparkFlex = motor; - encoder = ((SparkFlex) motor.getMotor()).getAbsoluteEncoder(); - motor.setAbsoluteEncoder(this); - motor.configureIntegratedEncoder(conversionFactor); - } else - { - throw new RuntimeException("Motor given to instantiate SparkFlexEncoder is not a CANSparkFlex"); - } - } - - @Override - public void close() - { - // SPARK Flex encoder gets closed with the motor - // I don't think an encoder getting closed should - // close the entire motor so i will keep this empty - // sparkFlex.close(); - } - - /** - * Reset the encoder to factory defaults. - */ - @Override - public void factoryDefault() - { - // Do nothing - } - - /** - * Clear sticky faults on the encoder. - */ - @Override - public void clearStickyFaults() - { - // Do nothing - } - - /** - * Configure the absolute encoder to read from [0, 360) per second. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) - { - if (sparkFlex instanceof SparkFlexSwerve) - { - SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig(); - cfg.absoluteEncoder.inverted(inverted); - ((SparkFlexSwerve) sparkFlex).updateConfig(cfg); - } - } - - /** - * Get the absolute position of the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - @Override - public double getAbsolutePosition() - { - return encoder.getPosition(); - } - - /** - * Get the instantiated absolute encoder Object. - * - * @return Absolute encoder object. - */ - @Override - public Object getAbsoluteEncoder() - { - return encoder; - } - - /** - * Sets the Absolute Encoder Offset inside of the SparkFlex's Memory. - * - * @param offset the offset the Absolute Encoder uses as the zero point. - * @return if setting Absolute Encoder Offset was successful or not. - */ - @Override - public boolean setAbsoluteEncoderOffset(double offset) - { - if (sparkFlex instanceof SparkFlexSwerve) - { - SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig(); - cfg.absoluteEncoder.zeroOffset(offset); - ((SparkFlexSwerve) sparkFlex).updateConfig(cfg); - return true; - } - return false; - } - - /** - * Get the velocity in degrees/sec. - * - * @return velocity in degrees/sec. - */ - @Override - public double getVelocity() - { - return encoder.getVelocity(); - } -} diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index da8edd050..12047ae29 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -1,554 +1,32 @@ package swervelib.motors; -import static edu.wpi.first.units.Units.Milliseconds; -import static edu.wpi.first.units.Units.Seconds; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.ClosedLoopSlot; -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 edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import java.util.Optional; -import java.util.function.Supplier; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.parser.PIDFConfig; -import swervelib.telemetry.SwerveDriveTelemetry; - -/** - * An implementation of {@link SparkFlex} as a {@link SwerveMotor}. - */ -public class SparkFlexSwerve extends SwerveMotor -{ - - /** - * Config retry delay. - */ - private final double configDelay = Milliseconds.of(5).in(Seconds); - /** - * {@link SparkFlex} Instance. - */ - private final SparkFlex motor; - /** - * Integrated encoder. - */ - public RelativeEncoder encoder; - /** - * Absolute encoder attached to the SparkFlex (if exists) - */ - public Optional absoluteEncoder = Optional.empty(); - /** - * Closed-loop PID controller. - */ - public SparkClosedLoopController pid; - /** - * Supplier for the velocity of the motor controller. - */ - private Supplier velocity; - /** - * Supplier for the position of the motor controller. - */ - private Supplier position; - /** - * An {@link Alert} for if there is an error configuring the motor. - */ - private Alert failureConfiguring; - /** - * Configuration object for {@link SparkFlex} motor. - */ - private SparkFlexConfig cfg = new SparkFlexConfig(); +/** An implementation of {@link SparkFlex} as a {@link SwerveMotor}. */ +public class SparkFlexSwerve extends SparkSwerve { /** * Initialize the swerve motor. * - * @param motor The SwerveMotor as a SparkFlex object. + * @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. + * @param motorType {@link DCMotor} which the {@link SparkFlex} is attached to. */ - public SparkFlexSwerve(SparkFlex motor, boolean isDriveMotor, DCMotor motorType) - { - this.motor = motor; - this.isDriveMotor = isDriveMotor; - factoryDefaults(); - clearStickyFaults(); - - encoder = motor.getEncoder(); - pid = motor.getClosedLoopController(); - cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder. - - // Spin off configurations in a different thread. - // configureSparkFlex(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. - failureConfiguring = new Alert("Motors", - "Failure configuring motor " + - motor.getDeviceId(), - AlertType.kWarning); - velocity = encoder::getVelocity; - position = encoder::getPosition; + public SparkFlexSwerve(SparkFlex motor, boolean isDriveMotor, DCMotor motorType) { + super(motor, new SparkFlexConfig(), isDriveMotor, motorType); } /** * Initialize the {@link SwerveMotor} as a {@link SparkFlex} connected to a Brushless Motor. * - * @param id CAN ID of the SparkFlex. + * @param id CAN ID of the SparkFlex. * @param isDriveMotor Is the motor being initialized a drive motor? - * @param motorType {@link DCMotor} which the {@link SparkFlex} is attached to. + * @param motorType {@link DCMotor} which the {@link SparkFlex} is attached to. */ - public SparkFlexSwerve(int id, boolean isDriveMotor, DCMotor motorType) - { + public SparkFlexSwerve(int id, boolean isDriveMotor, DCMotor motorType) { this(new SparkFlex(id, MotorType.kBrushless), isDriveMotor, motorType); } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkFlex(Supplier config) - { - for (int i = 0; i < maximumRetries; i++) - { - if (config.get() == REVLibError.kOk) - { - return; - } - Timer.delay(configDelay); - } - failureConfiguring.set(true); - } - - @Override - public void close() - { - motor.close(); - } - - /** - * Get the current configuration of the {@link SparkFlex} - * - * @return {@link SparkFlexConfig} - */ - 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) - { - if (!DriverStation.isDisabled()) - { - throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled."); - } - cfg.apply(cfgGiven); - configureSparkFlex(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters)); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) - { - cfg.voltageCompensation(nominalVoltage); - } - - /** - * 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.smartCurrentLimit(currentLimit); - } - - /** - * 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.closedLoopRampRate(rampRate) - .openLoopRampRate(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.getNeoVortex(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.isPresent(); - } - - /** - * Configure the factory defaults. - */ - @Override - public void factoryDefaults() - { - // Do nothing - } - - /** - * Clear the sticky faults on the motor controller. - */ - @Override - public void clearStickyFaults() - { - configureSparkFlex(motor::clearFaults); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - * @return The {@link SwerveMotor} for easy instantiation. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) - { - if (encoder == null) - { - this.absoluteEncoder = Optional.empty(); - cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); - - velocity = this.encoder::getVelocity; - position = this.encoder::getPosition; - } else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - this.absoluteEncoder = Optional.of(encoder); - - velocity = this.absoluteEncoder.get()::getVelocity; - position = this.absoluteEncoder.get()::getAbsolutePosition; - } - 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. - */ - @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.isEmpty()) - { - 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/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 - { - // 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.get().getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - - cfg.signals - .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs(20); - - cfg.absoluteEncoder - .positionConversionFactor(positionConversionFactor) - .velocityConversionFactor(positionConversionFactor / 60); - } - } - - } - - /** - * Configure the PIDF values for the closed loop controller. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) - { - cfg.closedLoop.pidf(config.p, config.i, config.d, config.f) - .iZone(config.iz) - .outputRange(config.output.min, 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.closedLoop - .positionWrappingEnabled(true) - .positionWrappingInputRange(minInput, maxInput); - - } - - /** - * Disable PID Wrapping on the motor. - */ - @Override - public void disablePIDWrapping() - { - cfg.closedLoop.positionWrappingEnabled(false); - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) - { - cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast); - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) - { - cfg.inverted(inverted); - } - - /** - * Save the configurations from flash to EEPROM. - */ - @Override - public void burnFlash() - { - if (!DriverStation.isDisabled()) - { - throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!"); - } - configureSparkFlex(() -> { - return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - }); - } - - /** - * 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) - { - - if (isDriveMotor) - { - configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kVelocity, - ClosedLoopSlot.kSlot0, - feedforward)); - } else - { - configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kPosition, - ClosedLoopSlot.kSlot0, - feedforward)); - if (SwerveDriveTelemetry.isSimulation) - { - encoder.setPosition(setpoint); - } - } - } - - /** - * 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) - { - setReference(setpoint, feedforward); - } - - /** - * Get the voltage output of the motor controller. - * - * @return Voltage output. - */ - @Override - public double getVoltage() - { - return motor.getAppliedOutput() * motor.getBusVoltage(); - } - - /** - * 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.getAppliedOutput(); - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity - */ - @Override - public double getVelocity() - { - return velocity.get(); - } - - /** - * Get the position of the integrated encoder. - * - * @return Position - */ - @Override - public double getPosition() - { - return position.get(); - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. - */ - @Override - public void setPosition(double position) - { - if (absoluteEncoder.isEmpty()) - { - configureSparkFlex(() -> encoder.setPosition(position)); - } - } - } diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index f2c7a40dc..bba190840 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -22,8 +22,9 @@ import edu.wpi.first.wpilibj.Timer; import java.util.Optional; import java.util.function.Supplier; -import swervelib.encoders.SparkMaxAnalogEncoderSwerve; -import swervelib.encoders.SparkMaxEncoderSwerve; + +import swervelib.encoders.SparkAnalogEncoderSwerve; +import swervelib.encoders.SparkEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; import swervelib.telemetry.SwerveDriveTelemetry; @@ -332,9 +333,9 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) noEncoderDefinedAlert.set(true); }); burnFlash(); - } else if (encoder instanceof SparkMaxAnalogEncoderSwerve || encoder instanceof SparkMaxEncoderSwerve) + } else if (encoder instanceof SparkAnalogEncoderSwerve || encoder instanceof SparkEncoderSwerve) { - cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve + cfg.closedLoop.feedbackSensor(encoder instanceof SparkAnalogEncoderSwerve ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder); this.absoluteEncoder = Optional.of(encoder); @@ -619,4 +620,4 @@ public enum Type */ kQuadrature, } -} +} \ No newline at end of file diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 8efcea84d..4644200c3 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -1,529 +1,32 @@ package swervelib.motors; -import static edu.wpi.first.units.Units.Milliseconds; -import static edu.wpi.first.units.Units.Seconds; - -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.ClosedLoopSlot; -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.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import java.util.Optional; -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 com.revrobotics.spark.SparkMax} as a {@link SwerveMotor}. - */ -public class SparkMaxSwerve extends SwerveMotor -{ - - /** - * Config retry delay. - */ - private final double configDelay = Milliseconds.of(5).in(Seconds); - /** - * {@link SparkMax} Instance. - */ - private final SparkMax motor; - /** - * Integrated encoder. - */ - public RelativeEncoder encoder; - /** - * Closed-loop PID controller. - */ - public SparkClosedLoopController pid; - /** - * Absolute encoder attached to the SparkMax (if exists) - */ - private Optional absoluteEncoder = Optional.empty(); - /** - * Supplier for the velocity of the motor controller. - */ - private Supplier velocity; - /** - * Supplier for the position of the motor controller. - */ - private Supplier position; - /** - * Configuration object for {@link SparkMax} motor. - */ - private SparkMaxConfig cfg = new SparkMaxConfig(); +/** An implementation of {@link com.revrobotics.spark.SparkMax} as a {@link SwerveMotor}. */ +public class SparkMaxSwerve extends SparkSwerve { /** * Initialize the swerve motor. * - * @param motor The SwerveMotor as a SparkMax object. + * @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. + * @param motorType Motor type controlled by the {@link SparkMax} motor controller. */ - public SparkMaxSwerve(SparkMax motor, boolean isDriveMotor, DCMotor motorType) - { - this.motor = motor; - this.isDriveMotor = isDriveMotor; - this.simMotor = motorType; - factoryDefaults(); - clearStickyFaults(); - - encoder = motor.getEncoder(); - pid = motor.getClosedLoopController(); - - cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder. - velocity = encoder::getVelocity; - position = encoder::getPosition; - - // Spin off configurations in a different thread. - // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. + public SparkMaxSwerve(SparkMax motor, boolean isDriveMotor, DCMotor motorType) { + super(motor, new SparkMaxConfig(), isDriveMotor, motorType); } - /** * Initialize the {@link SwerveMotor} as a {@link SparkMax} connected to a Brushless Motor. * - * @param id CAN ID of the SparkMax. + * @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. + * @param motorType Motor type controlled by the {@link SparkMax} motor controller. */ - public SparkMaxSwerve(int id, boolean isDriveMotor, DCMotor motorType) - { + public SparkMaxSwerve(int id, boolean isDriveMotor, DCMotor motorType) { this(new SparkMax(id, MotorType.kBrushless), isDriveMotor, motorType); } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - private void configureSparkMax(Supplier config) - { - for (int i = 0; i < maximumRetries; i++) - { - if (config.get() == REVLibError.kOk) - { - return; - } - Timer.delay(configDelay); - } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); - } - - @Override - public void close() - { - motor.close(); - } - - /** - * 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) - { - if (!DriverStation.isDisabled()) - { - throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled."); - } - cfg.apply(cfgGiven); - configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters)); - } - - /** - * Set the voltage compensation for the swerve module motor. - * - * @param nominalVoltage Nominal voltage for operation to output to. - */ - @Override - public void setVoltageCompensation(double nominalVoltage) - { - cfg.voltageCompensation(nominalVoltage); - } - - /** - * 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.smartCurrentLimit(currentLimit); - - } - - /** - * 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.closedLoopRampRate(rampRate) - .openLoopRampRate(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.getNEO(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.isPresent(); - } - - /** - * Configure the factory defaults. - */ - @Override - public void factoryDefaults() - { - // Do nothing - } - - /** - * Clear the sticky faults on the motor controller. - */ - @Override - public void clearStickyFaults() - { - configureSparkMax(motor::clearFaults); - } - - /** - * Set the absolute encoder to be a compatible absolute encoder. - * - * @param encoder The encoder to use. - * @return The {@link SwerveMotor} for easy instantiation. - */ - @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) - { - if (encoder == null) - { - this.absoluteEncoder = Optional.empty(); - cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); - - velocity = this.encoder::getVelocity; - position = this.encoder::getPosition; - burnFlash(); - } else if (encoder instanceof SparkMaxAnalogEncoderSwerve || encoder instanceof SparkMaxEncoderSwerve) - { - cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve - ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder); - - this.absoluteEncoder = Optional.of(encoder); - velocity = this.absoluteEncoder.get()::getVelocity; - position = this.absoluteEncoder.get()::getAbsolutePosition; - } - 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. - */ - @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); - - 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/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); - - - } - - /** - * Configure the PIDF values for the closed loop controller. - * - * @param config Configuration class holding the PIDF values. - */ - @Override - public void configurePIDF(PIDFConfig config) - { - cfg.closedLoop.pidf(config.p, config.i, config.d, config.f) - .iZone(config.iz) - .outputRange(config.output.min, 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.closedLoop - .positionWrappingEnabled(true) - .positionWrappingInputRange(minInput, maxInput); - - } - - /** - * Disable PID Wrapping on the motor. - */ - @Override - public void disablePIDWrapping() - { - cfg.closedLoop - .positionWrappingEnabled(false); - } - - /** - * Set the idle mode. - * - * @param isBrakeMode Set the brake mode. - */ - @Override - public void setMotorBrake(boolean isBrakeMode) - { - cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast); - - } - - /** - * Set the motor to be inverted. - * - * @param inverted State of inversion. - */ - @Override - public void setInverted(boolean inverted) - { - cfg.inverted(inverted); - } - - /** - * Save the configurations from flash to EEPROM. - */ - @Override - public void burnFlash() - { - if (!DriverStation.isDisabled()) - { - throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!"); - } - configureSparkMax(() -> { - return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - }); - } - - /** - * 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) - { - int pidSlot = 0; - - if (isDriveMotor) - { - configureSparkMax(() -> - pid.setReference( - setpoint, - ControlType.kVelocity, - ClosedLoopSlot.kSlot0, - feedforward)); - } else - { - configureSparkMax(() -> - pid.setReference( - setpoint, - ControlType.kPosition, - ClosedLoopSlot.kSlot0, - feedforward)); - if (SwerveDriveTelemetry.isSimulation) - { - encoder.setPosition(setpoint); - } - } - } - - /** - * 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) - { - setReference(setpoint, feedforward); - } - - /** - * Get the voltage output of the motor controller. - * - * @return Voltage output. - */ - @Override - public double getVoltage() - { - return motor.getAppliedOutput() * motor.getBusVoltage(); - } - - /** - * 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.getAppliedOutput(); - } - - /** - * Get the velocity of the integrated encoder. - * - * @return velocity - */ - @Override - public double getVelocity() - { - return velocity.get(); - } - - /** - * Get the position of the integrated encoder. - * - * @return Position - */ - @Override - public double getPosition() - { - return position.get(); - } - - /** - * Set the integrated encoder position. - * - * @param position Integrated encoder position. - */ - @Override - public void setPosition(double position) - { - if (absoluteEncoder.isEmpty()) - { - configureSparkMax(() -> encoder.setPosition(position)); - } - } } diff --git a/src/main/java/swervelib/motors/SparkSwerve.java b/src/main/java/swervelib/motors/SparkSwerve.java new file mode 100644 index 000000000..d64f63b67 --- /dev/null +++ b/src/main/java/swervelib/motors/SparkSwerve.java @@ -0,0 +1,507 @@ +package swervelib.motors; + +import static edu.wpi.first.units.Units.Milliseconds; +import static edu.wpi.first.units.Units.Seconds; + +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase; +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.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import java.util.Optional; +import java.util.function.Supplier; +import swervelib.encoders.SparkAnalogEncoderSwerve; +import swervelib.encoders.SparkEncoderSwerve; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.parser.PIDFConfig; +import swervelib.telemetry.SwerveDriveTelemetry; + +/** + * An implementation of {@link SparkBase} as a {@link SwerveMotor}. + */ +public class SparkSwerve extends SwerveMotor +{ + + /** + * Config retry delay. + */ + private final double configDelay = Milliseconds.of(5).in(Seconds); + + /** + * {@link SparkBase} Instance. + */ + private final SparkBase motor; + + /** + * Integrated encoder. + */ + public RelativeEncoder encoder; + + /** + * Closed-loop PID controller. + */ + public SparkClosedLoopController pid; + + /** + * Absolute encoder attached to the SparkBase (if exists) + */ + private Optional absoluteEncoder = Optional.empty(); + + /** + * Supplier for the velocity of the motor controller. + */ + private Supplier velocity; + + /** + * Supplier for the position of the motor controller. + */ + private Supplier position; + + /** + * Configuration object for {@link SparkBase} motor. + */ + private final SparkBaseConfig cfg; + + /** + * Initialize the swerve motor. + * + * @param motor The SwerveMotor as a SparkBase object. + * @param cfg Configuration for the SparkBase motor controller. + * @param isDriveMotor Is the motor being initialized a drive motor? + * @param motorType Motor type controlled by the {@link SparkBase} motor controller. + */ + public SparkSwerve(SparkBase motor, SparkBaseConfig cfg, boolean isDriveMotor, DCMotor motorType) + { + this.motor = motor; + this.cfg = cfg; + this.isDriveMotor = isDriveMotor; + this.simMotor = motorType; + factoryDefaults(); + clearStickyFaults(); + + encoder = motor.getEncoder(); + pid = motor.getClosedLoopController(); + + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder. + velocity = encoder::getVelocity; + position = encoder::getPosition; + + // Spin off configurations in a different thread. + // configureSparkBase(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. + } + + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkBase(Supplier config) + { + for (int i = 0; i < maximumRetries; i++) + { + if (config.get() == REVLibError.kOk) + { + return; + } + Timer.delay(configDelay); + } + DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + } + + @Override + public void close() + { + motor.close(); + } + + /** + * Get the current configuration of the {@link SparkBase} + * + * @return {@link SparkBaseConfig} + */ + public SparkBaseConfig getConfig() + { + return cfg; + } + + /** + * Update the config for the {@link SparkBase} + * + * @param cfgGiven Given {@link SparkBaseConfig} which should have minimal modifications. + */ + public void updateConfig(SparkBaseConfig cfgGiven) + { + if (!DriverStation.isDisabled()) + { + throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled."); + } + cfg.apply(cfgGiven); + configureSparkBase(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters)); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) + { + cfg.voltageCompensation(nominalVoltage); + } + + /** + * 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.smartCurrentLimit(currentLimit); + } + + /** + * 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.closedLoopRampRate(rampRate).openLoopRampRate(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.getNEO(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.isPresent(); + } + + /** + * Configure the factory defaults. + */ + @Override + public void factoryDefaults() + { + // Do nothing + } + + /** + * Clear the sticky faults on the motor controller. + */ + @Override + public void clearStickyFaults() + { + configureSparkBase(motor::clearFaults); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + * @return The {@link SwerveMotor} for easy instantiation. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) + { + if (encoder == null) + { + this.absoluteEncoder = Optional.empty(); + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + + velocity = this.encoder::getVelocity; + position = this.encoder::getPosition; + burnFlash(); + } else if (encoder instanceof SparkAnalogEncoderSwerve || encoder instanceof SparkEncoderSwerve) + { + cfg.closedLoop.feedbackSensor(encoder instanceof SparkAnalogEncoderSwerve + ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder); + + this.absoluteEncoder = Optional.of(encoder); + velocity = this.absoluteEncoder.get()::getVelocity; + position = this.absoluteEncoder.get()::getAbsolutePosition; + } + 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. + */ + @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); + + 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 integrated 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/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); + } + + /** + * Configure the PIDF values for the closed loop controller. + * + * @param config Configuration class holding the PIDF values. + */ + @Override + public void configurePIDF(PIDFConfig config) + { + cfg.closedLoop.pidf(config.p, config.i, config.d, config.f) + .iZone(config.iz) + .outputRange(config.output.min, 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.closedLoop + .positionWrappingEnabled(true) + .positionWrappingInputRange(minInput, maxInput); + } + + /** + * Disable PID Wrapping on the motor. + */ + @Override + public void disablePIDWrapping() + { + cfg.closedLoop.positionWrappingEnabled(false); + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) + { + cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) + { + cfg.inverted(inverted); + } + + /** + * Save the configurations from flash to EEPROM. + */ + @Override + public void burnFlash() + { + if (!DriverStation.isDisabled()) + { + throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!"); + } + configureSparkBase(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters)); + } + + /** + * 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) + { + if (isDriveMotor) + { + configureSparkBase(() -> + pid.setReference( + setpoint, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + feedforward)); + } else + { + configureSparkBase(() -> pid.setReference( + setpoint, + ControlType.kPosition, + ClosedLoopSlot.kSlot0, + feedforward)); + if (SwerveDriveTelemetry.isSimulation) + { + encoder.setPosition(setpoint); + } + } + } + + /** + * 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) + { + setReference(setpoint, feedforward); + } + + /** + * Get the voltage output of the motor controller. + * + * @return Voltage output. + */ + @Override + public double getVoltage() + { + return motor.getAppliedOutput() * motor.getBusVoltage(); + } + + /** + * 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.getAppliedOutput(); + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity + */ + @Override + public double getVelocity() + { + return velocity.get(); + } + + /** + * Get the position of the integrated encoder. + * + * @return Position + */ + @Override + public double getPosition() + { + return position.get(); + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. + */ + @Override + public void setPosition(double position) + { + if (absoluteEncoder.isEmpty()) { + configureSparkBase(() -> encoder.setPosition(position)); + } + } + +} diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 6960783f2..a0a67a2e9 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -12,9 +12,8 @@ import swervelib.encoders.CANCoderSwerve; import swervelib.encoders.CanAndMagSwerve; import swervelib.encoders.DIODutyCycleEncoderSwerve; -import swervelib.encoders.SparkFlexEncoderSwerve; -import swervelib.encoders.SparkMaxAnalogEncoderSwerve; -import swervelib.encoders.SparkMaxEncoderSwerve; +import swervelib.encoders.SparkAnalogEncoderSwerve; +import swervelib.encoders.SparkEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.encoders.TalonSRXEncoderSwerve; import swervelib.encoders.ThriftyNovaEncoderSwerve; @@ -78,16 +77,19 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) case "attached": case "canandmag": case "canandcoder": - return new SparkMaxEncoderSwerve(motor, 360); - case "sparkmax_analog": - return new SparkMaxAnalogEncoderSwerve(motor, 3.3); - case "sparkmax_analog5v": - return new SparkMaxAnalogEncoderSwerve(motor, 5); case "sparkflex_integrated": case "sparkflex_attached": case "sparkflex_canandmag": case "sparkflex_canandcoder": - return new SparkFlexEncoderSwerve(motor, 360); + return new SparkEncoderSwerve(motor, 360); + case "sparkmax_analog": + case "sparkflex_analog": + case "spark_analog": + return new SparkAnalogEncoderSwerve(motor, 3.3); + case "sparkmax_analog5v": + case "sparkflex_analog5v": + case "spark_analog5v": + return new SparkAnalogEncoderSwerve(motor, 5); case "canandcoder_can": case "canandmag_can": return new CanAndMagSwerve(id); diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index 9d00c88ea..ad09071c3 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -2,7 +2,7 @@ import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.util.Units; -import swervelib.encoders.SparkMaxEncoderSwerve; +import swervelib.encoders.SparkEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.encoders.ThriftyNovaEncoderSwerve; import swervelib.motors.SwerveMotor; @@ -113,7 +113,7 @@ public SwerveModuleConfiguration createModuleConfiguration( // Backwards compatibility, auto-optimization. if (conversionFactors.angle.factor == 360 && absEncoder != null && - (absEncoder instanceof SparkMaxEncoderSwerve && angleMotor.getMotor() instanceof SparkMax)) + (absEncoder instanceof SparkEncoderSwerve && angleMotor.getMotor() instanceof SparkMax)) { angleMotor.setAbsoluteEncoder(absEncoder); } else if ((absEncoder instanceof ThriftyNovaEncoderSwerve && angleMotor instanceof ThriftyNovaSwerve))