diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 18e1817fe..fa77501ed 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -22,6 +22,8 @@ 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; @@ -48,7 +50,7 @@ public class SparkFlexSwerve extends SwerveMotor /** * Absolute encoder attached to the SparkFlex (if exists) */ - public SwerveAbsoluteEncoder absoluteEncoder; + public Optional absoluteEncoder = Optional.empty(); /** * Closed-loop PID controller. */ @@ -224,7 +226,7 @@ public DCMotor getSimMotor() @Override public boolean isAttachedAbsoluteEncoder() { - return absoluteEncoder != null; + return absoluteEncoder.isPresent(); } /** @@ -256,7 +258,7 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { if (encoder == null) { - absoluteEncoder = null; + this.absoluteEncoder = Optional.empty(); cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); velocity = this.encoder::getVelocity; @@ -264,10 +266,10 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) } else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - absoluteEncoder = encoder; + this.absoluteEncoder = Optional.of(encoder); - velocity = absoluteEncoder::getVelocity; - position = absoluteEncoder::getAbsolutePosition; + velocity = this.absoluteEncoder.get()::getVelocity; + position = this.absoluteEncoder.get()::getAbsolutePosition; } return this; } @@ -294,7 +296,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) .appliedOutputPeriodMs(10) .faultsPeriodMs(20) ; - if (absoluteEncoder == null) + if (absoluteEncoder.isEmpty()) { cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); @@ -329,7 +331,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) // 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) + if (absoluteEncoder.get().getAbsoluteEncoder() instanceof AbsoluteEncoder) { cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); @@ -530,7 +532,7 @@ public double getPosition() @Override public void setPosition(double position) { - if (absoluteEncoder == null) + 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 afe02f68a..43a1df8c1 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -46,7 +46,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * Absolute encoder attached to the SparkMax (if exists) */ - public SwerveAbsoluteEncoder absoluteEncoder; + public Optional absoluteEncoder; /** * Integrated encoder. */ @@ -290,7 +290,7 @@ public DCMotor getSimMotor() @Override public boolean isAttachedAbsoluteEncoder() { - return absoluteEncoder != null; + return absoluteEncoder.isPresent(); } /** @@ -322,7 +322,7 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { if (encoder == null) { - absoluteEncoder = null; + this.absoluteEncoder = Optional.empty(); cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); this.encoder.ifPresentOrElse((RelativeEncoder enc) -> { @@ -337,12 +337,12 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder); - absoluteEncoder = encoder; - velocity = absoluteEncoder::getVelocity; - position = absoluteEncoder::getAbsolutePosition; + this.absoluteEncoder = Optional.of(encoder); + velocity = this.absoluteEncoder.get()::getVelocity; + position = this.absoluteEncoder.get()::getAbsolutePosition; noEncoderDefinedAlert.set(false); } - if (absoluteEncoder == null && this.encoder.isEmpty()) + if (absoluteEncoder.isEmpty() && this.encoder.isEmpty()) { noEncoderDefinedAlert.set(true); throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX"); @@ -371,7 +371,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) .iAccumulationAlwaysOn(false) .appliedOutputPeriodMs(10) .faultsPeriodMs(20); - if (absoluteEncoder == null) + if (absoluteEncoder.isEmpty()) { cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); cfg.encoder @@ -404,7 +404,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) // 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) + if (absoluteEncoder.get().getAbsoluteEncoder() instanceof AbsoluteEncoder) { cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); @@ -620,7 +620,7 @@ public double getPosition() @Override public void setPosition(double position) { - if (absoluteEncoder == null) + if (absoluteEncoder.isEmpty()) { encoder.ifPresent((RelativeEncoder enc) -> { configureSparkMax(() -> enc.setPosition(position)); diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 78db5b32a..ecfbb4dba 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -224,7 +224,7 @@ public DCMotor getSimMotor() @Override public boolean isAttachedAbsoluteEncoder() { - return absoluteEncoder != null; + return absoluteEncoder.isPresent(); } /** @@ -268,8 +268,8 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder); this.absoluteEncoder = Optional.of(encoder); - velocity = encoder::getVelocity; - position = encoder::getAbsolutePosition; + velocity = this.absoluteEncoder.get()::getVelocity; + position = this.absoluteEncoder.get()::getAbsolutePosition; } return this; }