diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java index 4c16c238..b42a635a 100644 --- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java +++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java @@ -50,6 +50,8 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; /** REV Spark */ public class Spark extends LoggableHardware { @@ -177,6 +179,10 @@ public static class SparkInputs { private FeedbackSensor m_feedbackSensor; private SparkLimitSwitch.Type m_limitSwitchType = SparkLimitSwitch.Type.kNormallyOpen; private RelativeEncoder m_encoder; + private Trigger m_forwardLimitSwitchTrigger; + private Trigger m_reverseLimitSwitchTrigger; + private double m_forwardLimitSwitchResetValue = 1; + private double m_reverseLimitSwitchResetValue = 0; private volatile SparkOutput m_output; private volatile SparkInputsAutoLogged m_inputs; @@ -208,6 +214,9 @@ 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_forwardLimitSwitchTrigger = new Trigger(() -> getInputs().forwardLimitSwitch); + this.m_reverseLimitSwitchTrigger = new Trigger(() -> getInputs().reverseLimitSwitch); + // Set CAN timeout m_spark.setCANTimeout(CAN_TIMEOUT_MS); @@ -567,6 +576,7 @@ private void updateInputs() { } } + /** * Handle smooth motion */ @@ -1175,6 +1185,46 @@ public REVLibError resetEncoder() { return resetEncoder(0.0); } + /** + * Sets whether the NEO encoder should be reset when the forward limit switch is hit + * The value to set it to can be configured using {@link Spark#setForwardLimitSwitchResetValue(double)} + * @param value Whether the encoder should be reset + */ + public void setForwardLimitSwitchShouldReset(boolean value) { + m_forwardLimitSwitchTrigger.onTrue(value ? + Commands.runOnce(() -> resetEncoder(m_forwardLimitSwitchResetValue)) : + Commands.none() + ); + } + + /** + * Change what value the encoder should be reset to once the forward limit switch is hit + * @param value Desired encoder value + */ + public void setForwardLimitSwitchResetValue(double value) { + m_forwardLimitSwitchResetValue = value; + } + + /** + * Sets whether the NEO encoder should be reset when the reverse limit switch is hit + * The value to set it to can be configured using {@link Spark#setReverseLimitSwitchResetValue(double)} + * @param value Whether the encoder should be reset + */ + public void setReverseLimitSwitchShouldReset(boolean value) { + m_reverseLimitSwitchTrigger.onTrue(value ? + Commands.runOnce(() -> resetEncoder(m_reverseLimitSwitchResetValue)) : + Commands.none() + ); + } + + /** + * Change what value the encoder should be reset to once the reverse limit switch is hit + * @param value Desired encoder value + */ + public void setReverseLimitSwitchResetValue(double value) { + m_reverseLimitSwitchResetValue = value; + } + /** * Disable forward limit switch * @return {@link REVLibError#kOk} if successful