From 9cd0e90de62079e383da7ae5ef55584717e6257d Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Fri, 30 Aug 2024 16:35:23 -0500 Subject: [PATCH 01/21] Fuse NEO encoder to external absolute encoder --- .../lasarobotics/drive/MAXSwerveModule.java | 17 ++++---- .../hardware/revrobotics/Spark.java | 41 +++++++++++++++---- 2 files changed, 42 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java b/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java index eca4d47f..7b63c699 100644 --- a/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java +++ b/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java @@ -206,9 +206,9 @@ public MAXSwerveModule(Hardware swerveHardware, ModuleLocation location, GearRat m_driveMotor.setVelocityConversionFactor(Spark.FeedbackSensor.NEO_ENCODER, m_driveConversionFactor / 60); // Set rotate encoder conversion factor - m_rotateConversionFactor = 2 * Math.PI; - m_rotateMotor.setPositionConversionFactor(Spark.FeedbackSensor.THROUGH_BORE_ENCODER, m_rotateConversionFactor); - m_rotateMotor.setVelocityConversionFactor(Spark.FeedbackSensor.THROUGH_BORE_ENCODER, m_rotateConversionFactor / 60); + m_rotateConversionFactor = 2 * Math.PI / DRIVE_ROTATE_GEAR_RATIO; + m_rotateMotor.setPositionConversionFactor(Spark.FeedbackSensor.ABSOLUTE_ENCODER, m_rotateConversionFactor); + m_rotateMotor.setVelocityConversionFactor(Spark.FeedbackSensor.ABSOLUTE_ENCODER, m_rotateConversionFactor / 60); // Enable PID wrapping m_rotateMotor.enablePIDWrapping(0.0, m_rotateConversionFactor); @@ -238,7 +238,10 @@ public MAXSwerveModule(Hardware swerveHardware, ModuleLocation location, GearRat // Initialize PID m_driveMotor.initializeSparkPID(m_driveMotorConfig, Spark.FeedbackSensor.NEO_ENCODER); - m_rotateMotor.initializeSparkPID(m_rotateMotorConfig, Spark.FeedbackSensor.THROUGH_BORE_ENCODER); + m_rotateMotor.initializeSparkPID(m_rotateMotorConfig, Spark.FeedbackSensor.FUSED_ENCODER); + + // Set gear ratio between motor and rotate encoder + m_rotateMotor.setMotorToSensorRatio(DRIVE_ROTATE_GEAR_RATIO); // Set drive motor to coast m_driveMotor.setIdleMode(IdleMode.kCoast); @@ -457,7 +460,7 @@ public void initSendable(SendableBuilder builder) { () -> m_rotateMotorConfig.getP(), (value) -> { m_rotateMotorConfig.setP(value); - m_rotateMotor.initializeSparkPID(m_rotateMotorConfig, FeedbackSensor.THROUGH_BORE_ENCODER); + m_rotateMotor.initializeSparkPID(m_rotateMotorConfig, FeedbackSensor.ABSOLUTE_ENCODER); } ); // Configure rotate kI @@ -466,7 +469,7 @@ public void initSendable(SendableBuilder builder) { () -> m_rotateMotorConfig.getI(), (value) -> { m_rotateMotorConfig.setI(value); - m_rotateMotor.initializeSparkPID(m_rotateMotorConfig, FeedbackSensor.THROUGH_BORE_ENCODER); + m_rotateMotor.initializeSparkPID(m_rotateMotorConfig, FeedbackSensor.ABSOLUTE_ENCODER); } ); // Configure rotate kD @@ -475,7 +478,7 @@ public void initSendable(SendableBuilder builder) { () -> m_rotateMotorConfig.getD(), (value) -> { m_rotateMotorConfig.setD(value); - m_rotateMotor.initializeSparkPID(m_rotateMotorConfig, FeedbackSensor.THROUGH_BORE_ENCODER); + m_rotateMotor.initializeSparkPID(m_rotateMotorConfig, FeedbackSensor.ABSOLUTE_ENCODER); } ); } diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java index baacf458..1d9a096a 100644 --- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java +++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java @@ -97,7 +97,7 @@ public double getMaxRPM() { /** Feedback sensor */ public enum FeedbackSensor { - NEO_ENCODER, ANALOG, THROUGH_BORE_ENCODER; + NEO_ENCODER, ANALOG, ABSOLUTE_ENCODER, FUSED_ENCODER; } /** Spark output */ @@ -180,6 +180,8 @@ public static class SparkInputs { private volatile SparkOutput m_output; private volatile SparkInputsAutoLogged m_inputs; + private volatile double m_motorToSensorRatio; + /** * Create a Spark that is unit-testing friendly with built-in logging * @param id Spark ID @@ -205,6 +207,7 @@ public Spark(ID id, MotorKind kind, SparkLimitSwitch.Type limitSwitchType, Measu this.m_limitSwitchType = limitSwitchType; this.m_parameterChain = new LinkedHashSet<>(); this.m_invertedRunner = () -> {}; + this.m_motorToSensorRatio = 1.0; // Set CAN timeout m_spark.setCANTimeout(CAN_TIMEOUT_MS); @@ -535,6 +538,7 @@ private REVLibError enableVoltageCompensation(Measure nominalVoltage) { */ private void updateInputs() { synchronized (m_inputs) { + // Get sensor inputs m_inputs.analogPosition = getAnalogPosition(); m_inputs.analogVelocity = getAnalogVelocity(); m_inputs.absoluteEncoderPosition = getAbsoluteEncoderPosition(); @@ -542,10 +546,18 @@ private void updateInputs() { m_inputs.forwardLimitSwitch = getForwardLimitSwitch().isPressed(); m_inputs.reverseLimitSwitch = getReverseLimitSwitch().isPressed(); - if (!getMotorType().equals(MotorType.kBrushed)) { + // Get motor encoder + if (!getMotorType().equals(MotorType.kBrushed) && !m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) { m_inputs.encoderPosition = getEncoderPosition(); m_inputs.encoderVelocity = getEncoderVelocity(); } + + // Fuse encoder if required + if (m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) { + m_inputs.encoderPosition = m_inputs.absoluteEncoderPosition * m_motorToSensorRatio; + m_inputs.encoderVelocity = m_inputs.absoluteEncoderVelocity * m_motorToSensorRatio; + resetEncoder(m_inputs.absoluteEncoderPosition * m_motorToSensorRatio); + } } } @@ -732,11 +744,12 @@ public void initializeSparkPID(SparkPIDConfig config, FeedbackSensor feedbackSen selectedSensor = getAnalog(); m_currentStateSupplier = () -> new TrapezoidProfile.State(getInputs().analogPosition, getInputs().analogVelocity); break; - case THROUGH_BORE_ENCODER: + case ABSOLUTE_ENCODER: selectedSensor = getAbsoluteEncoder(); m_currentStateSupplier = () -> new TrapezoidProfile.State(getInputs().absoluteEncoderPosition, getInputs().absoluteEncoderVelocity); break; case NEO_ENCODER: + case FUSED_ENCODER: default: selectedSensor = getEncoder(); m_currentStateSupplier = () -> new TrapezoidProfile.State(getInputs().encoderPosition, getInputs().encoderVelocity); @@ -879,6 +892,8 @@ public void set(double value, ControlType ctrl, double arbFeedforward, SparkPIDC * @return {@link REVLibError#kOk} if successful */ public REVLibError setPositionConversionFactor(FeedbackSensor sensor, double factor) { + if (sensor.equals(FeedbackSensor.FUSED_ENCODER)) + throw new IllegalArgumentException("Conversion factors must be set for motor encoder and absolute encoder separately when using fused mode!"); REVLibError status; Supplier parameterSetter; BooleanSupplier parameterCheckSupplier; @@ -891,7 +906,7 @@ public REVLibError setPositionConversionFactor(FeedbackSensor sensor, double fac parameterSetter = () -> getAnalog().setPositionConversionFactor(factor); parameterCheckSupplier = () -> Precision.equals(getAnalog().getPositionConversionFactor(), factor, EPSILON); break; - case THROUGH_BORE_ENCODER: + case ABSOLUTE_ENCODER: parameterSetter = () -> getAbsoluteEncoder().setPositionConversionFactor(factor); parameterCheckSupplier = () -> Precision.equals(getAbsoluteEncoder().getPositionConversionFactor(), factor, EPSILON); break; @@ -916,12 +931,11 @@ public REVLibError setPositionConversionFactor(FeedbackSensor sensor, double fac */ public double getPositionConversionFactor(FeedbackSensor sensor) { switch (sensor) { - case NEO_ENCODER: - return getEncoder().getPositionConversionFactor(); case ANALOG: return getAnalog().getPositionConversionFactor(); - case THROUGH_BORE_ENCODER: + case ABSOLUTE_ENCODER: return getAbsoluteEncoder().getPositionConversionFactor(); + case NEO_ENCODER: default: return getEncoder().getPositionConversionFactor(); } @@ -935,6 +949,8 @@ public double getPositionConversionFactor(FeedbackSensor sensor) { * @return {@link REVLibError#kOk} if successful */ public REVLibError setVelocityConversionFactor(FeedbackSensor sensor, double factor) { + if (sensor.equals(FeedbackSensor.FUSED_ENCODER)) + throw new IllegalArgumentException("Conversion factors must be set for motor encoder and absolute encoder separately when using fused mode!"); REVLibError status; Supplier parameterSetter; BooleanSupplier parameterCheckSupplier; @@ -947,7 +963,7 @@ public REVLibError setVelocityConversionFactor(FeedbackSensor sensor, double fac parameterSetter = () -> getAnalog().setVelocityConversionFactor(factor); parameterCheckSupplier = () -> Precision.equals(getAnalog().getVelocityConversionFactor(), factor, EPSILON); break; - case THROUGH_BORE_ENCODER: + case ABSOLUTE_ENCODER: parameterSetter = () -> getAbsoluteEncoder().setVelocityConversionFactor(factor); parameterCheckSupplier = () -> Precision.equals(getAnalog().getVelocityConversionFactor(), factor, EPSILON); break; @@ -976,7 +992,7 @@ public double getVelocityConversionFactor(FeedbackSensor sensor) { return getEncoder().getVelocityConversionFactor(); case ANALOG: return getAnalog().getVelocityConversionFactor(); - case THROUGH_BORE_ENCODER: + case ABSOLUTE_ENCODER: return getAbsoluteEncoder().getVelocityConversionFactor(); default: return getEncoder().getVelocityConversionFactor(); @@ -1106,6 +1122,13 @@ public void smoothMotion(double value, TrapezoidProfile.Constraints motionConstr smoothMotion(value, motionConstraint, (motionProfileState) -> 0.0); } + /** + * Set gear ratio between motor shaft and external absolute encoder + * @param ratio Gear ratio + */ + public void setMotorToSensorRatio(double ratio) { + m_motorToSensorRatio = ratio; + } /** * Reset NEO built-in encoder to desired value From c7dfc7d0c022fd658b38b24fa584600b99d0ed00 Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Fri, 30 Aug 2024 16:41:53 -0500 Subject: [PATCH 02/21] Initialize feedback sensor --- src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java index 1d9a096a..0b48cdf7 100644 --- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java +++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java @@ -207,6 +207,7 @@ public Spark(ID id, MotorKind kind, SparkLimitSwitch.Type limitSwitchType, Measu this.m_limitSwitchType = limitSwitchType; this.m_parameterChain = new LinkedHashSet<>(); this.m_invertedRunner = () -> {}; + this.m_feedbackSensor = FeedbackSensor.NEO_ENCODER; this.m_motorToSensorRatio = 1.0; // Set CAN timeout From 524bc4a4cafc5757c15bd873fe8d98646247ba8a Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Fri, 30 Aug 2024 16:49:12 -0500 Subject: [PATCH 03/21] Do not set sensor inversion in fused mode --- src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java index 0b48cdf7..67aa48c0 100644 --- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java +++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java @@ -759,7 +759,7 @@ public void initializeSparkPID(SparkPIDConfig config, FeedbackSensor feedbackSen // Configure feedback sensor and set sensor phase m_spark.getPIDController().setFeedbackDevice(selectedSensor); - if (!m_feedbackSensor.equals(FeedbackSensor.NEO_ENCODER)) { + if (!m_feedbackSensor.equals(FeedbackSensor.NEO_ENCODER) || !m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) { parameterApplier = () -> applyParameter( () -> selectedSensor.setInverted(m_config.getSensorPhase()), () -> selectedSensor.getInverted() == m_config.getSensorPhase(), From 4910d304fa1e7ba1e2b9ea74315f61ec6b3e3320 Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Fri, 30 Aug 2024 16:52:02 -0500 Subject: [PATCH 04/21] Fix conditional --- src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java index 67aa48c0..f2cc52bb 100644 --- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java +++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java @@ -759,7 +759,7 @@ public void initializeSparkPID(SparkPIDConfig config, FeedbackSensor feedbackSen // Configure feedback sensor and set sensor phase m_spark.getPIDController().setFeedbackDevice(selectedSensor); - if (!m_feedbackSensor.equals(FeedbackSensor.NEO_ENCODER) || !m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) { + if (!m_feedbackSensor.equals(FeedbackSensor.NEO_ENCODER) && !m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) { parameterApplier = () -> applyParameter( () -> selectedSensor.setInverted(m_config.getSensorPhase()), () -> selectedSensor.getInverted() == m_config.getSensorPhase(), From ecd9a1465cd8b21bec3c166efe111a3054f571a0 Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Fri, 30 Aug 2024 17:12:52 -0500 Subject: [PATCH 05/21] Code cleanup --- .../hardware/revrobotics/Spark.java | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java index f2cc52bb..6a2fc87e 100644 --- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java +++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java @@ -534,6 +534,21 @@ private REVLibError enableVoltageCompensation(Measure nominalVoltage) { return status; } + /** + * Fuse absolute encoder to NEO built-in encoder + * @return {@link REVLibError#kOk} if successful + */ + private REVLibError fuseEncoder() { + // Fuse encoder if required + if (m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) { + m_inputs.encoderPosition = m_inputs.absoluteEncoderPosition * m_motorToSensorRatio; + m_inputs.encoderVelocity = m_inputs.absoluteEncoderVelocity * m_motorToSensorRatio; + getEncoder().setPosition(m_inputs.absoluteEncoderPosition * m_motorToSensorRatio); + } + + return REVLibError.kOk; + } + /** * Update sensor input readings */ @@ -554,11 +569,7 @@ private void updateInputs() { } // Fuse encoder if required - if (m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) { - m_inputs.encoderPosition = m_inputs.absoluteEncoderPosition * m_motorToSensorRatio; - m_inputs.encoderVelocity = m_inputs.absoluteEncoderVelocity * m_motorToSensorRatio; - resetEncoder(m_inputs.absoluteEncoderPosition * m_motorToSensorRatio); - } + fuseEncoder(); } } From 85de33268d9df6619ed9a143402a32cdd50f98d2 Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Fri, 30 Aug 2024 17:43:36 -0500 Subject: [PATCH 06/21] Set Spark to desired position once smooth motion is finished --- .../org/lasarobotics/hardware/revrobotics/Spark.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java index 6a2fc87e..7a462dc3 100644 --- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java +++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java @@ -588,6 +588,15 @@ private void handleSmoothMotion() { ); m_isSmoothMotionEnabled = !isSmoothMotionFinished(); + + if (!m_isSmoothMotionEnabled) { + set( + m_desiredState.position, + ControlType.kPosition, + m_feedforwardSupplier.apply(m_desiredState), + SparkPIDController.ArbFFUnits.kVoltage + ); + } } /** From 4cef4c5cd5f98baedafd6ed3aee6117c961fe676 Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Fri, 30 Aug 2024 19:28:23 -0500 Subject: [PATCH 07/21] Fix conversion factors --- .../org/lasarobotics/drive/MAXSwerveModule.java | 2 +- .../lasarobotics/hardware/revrobotics/Spark.java | 15 ++++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java b/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java index 7b63c699..14eadf9b 100644 --- a/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java +++ b/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java @@ -206,7 +206,7 @@ public MAXSwerveModule(Hardware swerveHardware, ModuleLocation location, GearRat m_driveMotor.setVelocityConversionFactor(Spark.FeedbackSensor.NEO_ENCODER, m_driveConversionFactor / 60); // Set rotate encoder conversion factor - m_rotateConversionFactor = 2 * Math.PI / DRIVE_ROTATE_GEAR_RATIO; + m_rotateConversionFactor = 2 * Math.PI; m_rotateMotor.setPositionConversionFactor(Spark.FeedbackSensor.ABSOLUTE_ENCODER, m_rotateConversionFactor); m_rotateMotor.setVelocityConversionFactor(Spark.FeedbackSensor.ABSOLUTE_ENCODER, m_rotateConversionFactor / 60); diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java index 7a462dc3..e42ed931 100644 --- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java +++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java @@ -180,8 +180,6 @@ public static class SparkInputs { private volatile SparkOutput m_output; private volatile SparkInputsAutoLogged m_inputs; - private volatile double m_motorToSensorRatio; - /** * Create a Spark that is unit-testing friendly with built-in logging * @param id Spark ID @@ -208,7 +206,6 @@ public Spark(ID id, MotorKind kind, SparkLimitSwitch.Type limitSwitchType, Measu this.m_parameterChain = new LinkedHashSet<>(); this.m_invertedRunner = () -> {}; this.m_feedbackSensor = FeedbackSensor.NEO_ENCODER; - this.m_motorToSensorRatio = 1.0; // Set CAN timeout m_spark.setCANTimeout(CAN_TIMEOUT_MS); @@ -541,9 +538,9 @@ private REVLibError enableVoltageCompensation(Measure nominalVoltage) { private REVLibError fuseEncoder() { // Fuse encoder if required if (m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) { - m_inputs.encoderPosition = m_inputs.absoluteEncoderPosition * m_motorToSensorRatio; - m_inputs.encoderVelocity = m_inputs.absoluteEncoderVelocity * m_motorToSensorRatio; - getEncoder().setPosition(m_inputs.absoluteEncoderPosition * m_motorToSensorRatio); + m_inputs.encoderPosition = m_inputs.absoluteEncoderPosition; + m_inputs.encoderVelocity = m_inputs.absoluteEncoderVelocity; + getEncoder().setPosition(m_inputs.absoluteEncoderPosition); } return REVLibError.kOk; @@ -777,6 +774,9 @@ public void initializeSparkPID(SparkPIDConfig config, FeedbackSensor feedbackSen break; } + // If fused, seed NEO encoder with absolute + if (m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) resetEncoder(getAbsoluteEncoderPosition()); + // Configure feedback sensor and set sensor phase m_spark.getPIDController().setFeedbackDevice(selectedSensor); if (!m_feedbackSensor.equals(FeedbackSensor.NEO_ENCODER) && !m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) { @@ -1148,7 +1148,8 @@ public void smoothMotion(double value, TrapezoidProfile.Constraints motionConstr * @param ratio Gear ratio */ public void setMotorToSensorRatio(double ratio) { - m_motorToSensorRatio = ratio; + setPositionConversionFactor(FeedbackSensor.NEO_ENCODER, getPositionConversionFactor(FeedbackSensor.NEO_ENCODER) / ratio); + setVelocityConversionFactor(FeedbackSensor.NEO_ENCODER, getVelocityConversionFactor(FeedbackSensor.NEO_ENCODER) / ratio); } /** From 21aebc4078e0d938b021b54c81c978028f2a5ec8 Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Sat, 31 Aug 2024 02:27:10 -0500 Subject: [PATCH 08/21] Warn when user sets motor to sensor ratio without using fused mode --- .../org/lasarobotics/hardware/revrobotics/Spark.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java index e42ed931..d2b3f225 100644 --- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java +++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java @@ -1148,8 +1148,15 @@ public void smoothMotion(double value, TrapezoidProfile.Constraints motionConstr * @param ratio Gear ratio */ public void setMotorToSensorRatio(double ratio) { - setPositionConversionFactor(FeedbackSensor.NEO_ENCODER, getPositionConversionFactor(FeedbackSensor.NEO_ENCODER) / ratio); - setVelocityConversionFactor(FeedbackSensor.NEO_ENCODER, getVelocityConversionFactor(FeedbackSensor.NEO_ENCODER) / ratio); + if (!m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) { + org.tinylog.Logger.tag(LOG_TAG) + .warn( + "Feedback sensor is not set to fused, currently set to {}", + m_feedbackSensor.toString() + ); + } + setPositionConversionFactor(FeedbackSensor.NEO_ENCODER, getPositionConversionFactor(FeedbackSensor.ABSOLUTE_ENCODER) / ratio); + setVelocityConversionFactor(FeedbackSensor.NEO_ENCODER, getVelocityConversionFactor(FeedbackSensor.ABSOLUTE_ENCODER) / ratio); } /** From 60404b820fcc1971234f9427d8b11241049c8768 Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Sat, 31 Aug 2024 23:49:17 -0500 Subject: [PATCH 09/21] Automatically increase status frame rate when using fused mode --- .../hardware/revrobotics/Spark.java | 38 ++++++++++++------- 1 file changed, 24 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java index d2b3f225..99bce07f 100644 --- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java +++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java @@ -158,7 +158,8 @@ public static class SparkInputs { private ID m_id; private MotorKind m_kind; - private Notifier m_inputsThread; + private Notifier m_inputThread; + private Measure