From ddd58e02d0025c485f055745feff7308361030bf Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Fri, 10 Jan 2025 16:40:18 -0600 Subject: [PATCH] Optimized and fixed absolute encoder inversion for SparkMAX attached encoders. Signed-off-by: thenetworkgrinch --- .../swervelib/encoders/SparkMaxAnalogEncoderSwerve.java | 4 ++-- src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java | 6 +++--- src/main/java/swervelib/motors/SparkFlexSwerve.java | 6 +++++- .../java/swervelib/motors/SparkMaxBrushedMotorSwerve.java | 6 +++++- src/main/java/swervelib/motors/SparkMaxSwerve.java | 6 +++++- 5 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index aa88efcf..1cea8305 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -110,12 +110,12 @@ public void configure(boolean inverted) if (sparkMax instanceof SparkMaxSwerve) { SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); - cfg.analogSensor.inverted(true); + cfg.analogSensor.inverted(inverted); ((SparkMaxSwerve) sparkMax).updateConfig(cfg); } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) { SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); - cfg.analogSensor.inverted(true); + cfg.analogSensor.inverted(inverted); ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); } } diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index b01a39f1..456203fe 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -46,7 +46,7 @@ public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) { failureConfiguring = new Alert( "Encoders", - "Failure configuring SparkMax Analog Encoder", + "Failure configuring SparkMax Absolute Encoder", AlertType.kWarning); offsetFailure = new Alert( "Encoders", @@ -110,12 +110,12 @@ public void configure(boolean inverted) if (sparkMax instanceof SparkMaxSwerve) { SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); - cfg.analogSensor.inverted(true); + cfg.absoluteEncoder.inverted(inverted); ((SparkMaxSwerve) sparkMax).updateConfig(cfg); } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) { SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); - cfg.analogSensor.inverted(true); + cfg.absoluteEncoder.inverted(inverted); ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); } } diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index d584f527..18e1817f 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -33,6 +33,10 @@ public class SparkFlexSwerve extends SwerveMotor { + /** + * Config retry delay. + */ + private final double configDelay = Milliseconds.of(5).in(Seconds); /** * {@link SparkFlex} Instance. */ @@ -120,7 +124,7 @@ private void configureSparkFlex(Supplier config) { return; } - Timer.delay(Milliseconds.of(5).in(Seconds)); + Timer.delay(configDelay); } failureConfiguring.set(true); } diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index be94485b..afe02f68 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -35,6 +35,10 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor { + /** + * Config retry delay. + */ + private final double configDelay = Milliseconds.of(5).in(Seconds); /** * SparkMAX Instance. */ @@ -187,7 +191,7 @@ private void configureSparkMax(Supplier config) { return; } - Timer.delay(Milliseconds.of(5).in(Seconds)); + Timer.delay(configDelay); } failureConfiguringAlert.set(true); } diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 383d84b1..78db5b32 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -33,6 +33,10 @@ public class SparkMaxSwerve extends SwerveMotor { + /** + * Config retry delay. + */ + private final double configDelay = Milliseconds.of(5).in(Seconds); /** * {@link SparkMax} Instance. */ @@ -119,7 +123,7 @@ private void configureSparkMax(Supplier config) { return; } - Timer.delay(Milliseconds.of(5).in(Seconds)); + Timer.delay(configDelay); } DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); }