diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 15312cea..202a64d6 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -1,5 +1,7 @@ package swervelib; +import com.revrobotics.CANSparkMax; +import com.revrobotics.MotorFeedbackSensor; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -591,13 +593,23 @@ public void pushOffsetsToEncoders() { if (absoluteEncoder != null && angleOffset == configuration.angleOffset) { - if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) + // If the absolute encoder is attached. + if (angleMotor.getMotor() instanceof CANSparkMax) { - angleOffset = 0; - } else - { - encoderOffsetWarning.set(true); + if (absoluteEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + { + angleMotor.setAbsoluteEncoder(absoluteEncoder); + if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) + { + angleOffset = 0; + } else + { + angleMotor.setAbsoluteEncoder(null); + encoderOffsetWarning.set(true); + } + } } + } else { noEncoderWarning.set(true); diff --git a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java index 1c6b7b13..ba977975 100644 --- a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java @@ -1,26 +1,26 @@ package swervelib.encoders; -import com.reduxrobotics.sensors.canandcoder.Canandcoder; +import com.reduxrobotics.sensors.canandmag.Canandmag; /** - * HELIUM {@link Canandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus. + * HELIUM {@link Canandmag} from ReduxRobotics absolute encoder, attached through the CAN bus. */ public class CanAndCoderSwerve extends SwerveAbsoluteEncoder { /** - * The {@link Canandcoder} representing the CANandCoder on the CAN bus. + * The {@link Canandmag} representing the CANandCoder on the CAN bus. */ - public Canandcoder encoder; + public Canandmag encoder; /** - * Create the {@link Canandcoder} + * Create the {@link Canandmag} * * @param canid The CAN ID whenever the CANandCoder is operating on the CANBus. */ public CanAndCoderSwerve(int canid) { - encoder = new Canandcoder(canid); + encoder = new Canandmag(canid); } /** @@ -51,7 +51,7 @@ public void clearStickyFaults() @Override public void configure(boolean inverted) { - encoder.setSettings(new Canandcoder.Settings().setInvertDirection(inverted)); + encoder.setSettings(new Canandmag.Settings().setInvertDirection(inverted)); } /** @@ -85,7 +85,7 @@ public Object getAbsoluteEncoder() @Override public boolean setAbsoluteEncoderOffset(double offset) { - return encoder.setSettings(new Canandcoder.Settings().setZeroOffset(offset)); + return encoder.setSettings(new Canandmag.Settings().setZeroOffset(offset)); } /** diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 0c96826a..40a27702 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -200,7 +200,11 @@ public void clearStickyFaults() @Override public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + if (encoder == null) + { + absoluteEncoder = null; + configureSparkFlex(() -> pid.setFeedbackDevice(this.encoder)); + } else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { absoluteEncoderOffsetWarning.set(true); absoluteEncoder = encoder; diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index bb33fc4c..d8fb2a8a 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -237,7 +237,11 @@ public void clearStickyFaults() @Override public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) + if (encoder == null) + { + absoluteEncoder = null; + configureSparkMax(() -> pid.setFeedbackDevice(this.encoder)); + } else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder(); configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder)); diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index aa19a0cf..a2c421e1 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -192,10 +192,16 @@ public void clearStickyFaults() @Override public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { - if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + if (encoder == null) + { + absoluteEncoder = null; + configureSparkMax(() -> pid.setFeedbackDevice(this.encoder)); + velocity = this.encoder::getVelocity; + position = this.encoder::getPosition; + } else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { DriverStation.reportWarning( - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" + + "IF possible configure the encoder offset in the REV Hardware Client instead of using the" + " absoluteEncoderOffset in the Swerve Module JSON!", false); absoluteEncoder = encoder; diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index 5eb34713..4e89652b 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -81,15 +81,6 @@ public SwerveModuleConfiguration createModuleConfiguration( SwerveMotor angleMotor = angle.createMotor(false); SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor); - // If the absolute encoder is attached. - if (absEncoder != null && angleMotor.getMotor() instanceof CANSparkMax) - { - if (absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) - { - angleMotor.setAbsoluteEncoder(absEncoder); - } - } - // Setup deprecation notice. // if (this.conversionFactor.drive != 0 && this.conversionFactor.angle != 0) // {