diff --git a/strongback/src/org/strongback/components/TalonSRX.java b/strongback/src/org/strongback/components/TalonSRX.java index 013b9aa..74388e9 100644 --- a/strongback/src/org/strongback/components/TalonSRX.java +++ b/strongback/src/org/strongback/components/TalonSRX.java @@ -140,34 +140,56 @@ public default AngleSensor getAngleSensor() { public TalonSRX reverseSensor(boolean flip); /** - * Set the soft limit for forward motion, which will disable the motor when the sensor is out of range. + * Set the soft limit for the forward throttle, in terms of the angle as measured by the {@link #getSelectedSensor() + * selected input sensor}. Soft limits can be used to disable motor drive when the "Sensor Position" is outside of a + * specified range: forward throttle will be disabled if the "Sensor Position" is greater than the forward soft limit. + * This takes effect only when the forward soft limit is {@link #enableForwardSoftLimit(boolean)}. + *

+ * Soft limits can be used to disable motor drive when the “Sensor Position” is outside of a specified range. Forward + * throttle will be disabled if the “Sensor Position” is greater than the Forward Soft Limit. Reverse throttle will be + * disabled if the “Sensor Position” is less than the Reverse Soft Limit. The respective Soft Limit Enable must be enabled + * for this feature to take effect. * - * @param forwardLimit the sensor limit beyond which the motor's forward direction should be halted. + * @param forwardLimitInDegrees the angle at which the forward throttle should be disabled, where the angle in terms of the + * {@link #getSelectedSensor()} * @return this object so that methods can be chained; never null + * @see #enableForwardSoftLimit(boolean) */ - public TalonSRX setForwardSoftLimit(int forwardLimit); + public TalonSRX setForwardSoftLimit(int forwardLimitInDegrees); /** - * Enable the forward soft limit. + * Set the soft limit for the reverse throttle, in terms of the angle as measured by the {@link #getSelectedSensor() + * selected input sensor}. Soft limits can be used to disable motor drive when the "Sensor Position" it outside of a + * specified range: reverse throttle will be disabled if the "Sensor Position" is less than the reverse soft limit. + * This takes effect only when the reverse soft limit is {@link #enableReverseSoftLimit(boolean)}. + *

+ * Soft limits can be used to disable motor drive when the “Sensor Position” is outside of a specified range. Forward + * throttle will be disabled if the “Sensor Position” is greater than the Forward Soft Limit. Reverse throttle will be + * disabled if the “Sensor Position” is less than the Reverse Soft Limit. The respective Soft Limit Enable must be enabled + * for this feature to take effect. * - * @param enable true if the limit is to be enabled, or false otherwise + * @param reverseLimitInDegrees the angle at which the reverse throttle should be disabled, where the angle in terms of the + * {@link #getSelectedSensor()} * @return this object so that methods can be chained; never null + * @see #enableForwardSoftLimit(boolean) */ - public TalonSRX enableForwardSoftLimit(boolean enable); + public TalonSRX setReverseSoftLimit(int reverseLimitInDegrees); /** - * Set the soft limit for reverse motion, which will disable the motor when the sensor is out of range. + * Enable the soft limit for forward throttle, which is set via the {@link #setForwardSoftLimit(int)}. * - * @param reverseLimit the sensor limit beyond which the motor's reverse direction should be halted. + * @param enable {@code true} if the forward throttle soft limit should be enabled * @return this object so that methods can be chained; never null + * @see #setForwardLimitSwitchNormallyOpen(boolean) */ - public TalonSRX setReverseSoftLimit(int reverseLimit); + public TalonSRX enableForwardSoftLimit(boolean enable); /** - * Enable the reverse soft limit. + * Enable the soft limit for reverse throttle, which is set via the {@link #setReverseSoftLimit(int)}. * - * @param enable true if the limit is to be enabled, or false otherwise + * @param enable {@code true} if the forward throttle soft limit should be enabled * @return this object so that methods can be chained; never null + * @see #setReverseLimitSwitchNormallyOpen(boolean) */ public TalonSRX enableReverseSoftLimit(boolean enable); diff --git a/strongback/src/org/strongback/control/TalonController.java b/strongback/src/org/strongback/control/TalonController.java index 4a6f044..ddbd99a 100644 --- a/strongback/src/org/strongback/control/TalonController.java +++ b/strongback/src/org/strongback/control/TalonController.java @@ -212,13 +212,13 @@ public int value() { public TalonController reverseOutput(boolean flip); @Override - public TalonController setForwardSoftLimit(int forwardLimit); + public TalonController enableForwardSoftLimit(boolean enable); @Override - public TalonController enableForwardSoftLimit(boolean enable); + public TalonController setForwardSoftLimit(int forwardLimitInDegrees); @Override - public TalonController setReverseSoftLimit(int reverseLimit); + public TalonController setReverseSoftLimit(int reverseLimitInDegrees); @Override public TalonController enableReverseSoftLimit(boolean enable); diff --git a/strongback/src/org/strongback/hardware/HardwareTalonController.java b/strongback/src/org/strongback/hardware/HardwareTalonController.java index 500d3dc..efedd2f 100644 --- a/strongback/src/org/strongback/hardware/HardwareTalonController.java +++ b/strongback/src/org/strongback/hardware/HardwareTalonController.java @@ -197,68 +197,68 @@ public TalonController reverseSensor(boolean flip) { } @Override - public TalonController setForwardSoftLimit(int forwardLimit) { - super.setForwardSoftLimit(forwardLimit); + public TalonController setForwardSoftLimit(int forwardLimitDegrees) { + super.setForwardSoftLimit(forwardLimitDegrees); return this; } @Override - public TalonController enableForwardSoftLimit(boolean enable) { + public HardwareTalonController enableForwardSoftLimit(boolean enable) { super.enableForwardSoftLimit(enable); return this; } @Override - public TalonController setReverseSoftLimit(int reverseLimit) { - super.setReverseSoftLimit(reverseLimit); + public HardwareTalonController setReverseSoftLimit(int reverseLimitDegrees) { + super.setReverseSoftLimit(reverseLimitDegrees); return this; } @Override - public TalonController enableReverseSoftLimit(boolean enable) { + public HardwareTalonController enableReverseSoftLimit(boolean enable) { super.enableReverseSoftLimit(enable); return this; } @Override - public TalonController enableLimitSwitch(boolean forward, boolean reverse) { + public HardwareTalonController enableLimitSwitch(boolean forward, boolean reverse) { super.enableLimitSwitch(forward, reverse); return this; } @Override - public TalonController enableBrakeMode(boolean brake) { + public HardwareTalonController enableBrakeMode(boolean brake) { super.enableBrakeMode(brake); return this; } @Override - public TalonController setForwardLimitSwitchNormallyOpen(boolean normallyOpen) { + public HardwareTalonController setForwardLimitSwitchNormallyOpen(boolean normallyOpen) { super.setForwardLimitSwitchNormallyOpen(normallyOpen); return this; } @Override - public TalonController setReverseLimitSwitchNormallyOpen(boolean normallyOpen) { + public HardwareTalonController setReverseLimitSwitchNormallyOpen(boolean normallyOpen) { super.setReverseLimitSwitchNormallyOpen(normallyOpen); return this; } @Override - public TalonController withGains(double p, double i, double d) { + public HardwareTalonController withGains(double p, double i, double d) { talon.setPID(p, i, d); return this; } @Override - public TalonController withGains(double p, double i, double d, double feedForward) { + public HardwareTalonController withGains(double p, double i, double d, double feedForward) { talon.setPID(p, i, d); talon.setF(feedForward); return this; } @Override - public TalonController withGains(double p, double i, double d, double feedForward, int izone, double closeLoopRampRate) { + public HardwareTalonController withGains(double p, double i, double d, double feedForward, int izone, double closeLoopRampRate) { talon.setPID(p, i, d); talon.setF(feedForward); talon.setIZone(izone); @@ -267,17 +267,17 @@ public TalonController withGains(double p, double i, double d, double feedForwar } @Override - public TalonController withProfile(int profile, double p, double i, double d) { + public HardwareTalonController withProfile(int profile, double p, double i, double d) { return withProfile(profile,p,i,d,0.0,0,0.0); } @Override - public TalonController withProfile(int profile, double p, double i, double d, double feedForward) { + public HardwareTalonController withProfile(int profile, double p, double i, double d, double feedForward) { return withProfile(profile,p,i,d,feedForward,0,0.0); } @Override - public TalonController withProfile(int profile, double p, double i, double d, double feedForward, int izone, double closeLoopRampRate) { + public HardwareTalonController withProfile(int profile, double p, double i, double d, double feedForward, int izone, double closeLoopRampRate) { talon.setPID(p, i, d, feedForward, izone, closeLoopRampRate, profile); return this; } diff --git a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java index 368a330..f642dc0 100644 --- a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java +++ b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java @@ -441,25 +441,33 @@ public TemperatureSensor getTemperatureSensor() { } @Override - public TalonSRX setForwardSoftLimit(int forwardLimit) { - talon.setForwardSoftLimit(forwardLimit); + public TalonSRX setForwardSoftLimit(int forwardLimitDegrees) { + // Compute the desired forward limit in terms of the current selected input sensor ... + if ( this.selectedInput != null ) { + double rawPosition = this.selectedInput.rawPositionForAngleInDegrees(forwardLimitDegrees); + talon.setForwardSoftLimit(rawPosition); + } return this; } @Override - public TalonSRX enableForwardSoftLimit(boolean enable) { + public HardwareTalonSRX enableForwardSoftLimit(boolean enable) { talon.enableForwardSoftLimit(enable); return this; } @Override - public TalonSRX setReverseSoftLimit(int reverseLimit) { - talon.setReverseSoftLimit(reverseLimit); + public HardwareTalonSRX setReverseSoftLimit(int reverseLimitDegrees) { + // Compute the desired reverse limit in terms of the current selected input sensor ... + if ( this.selectedInput != null ) { + double rawPosition = this.selectedInput.rawPositionForAngleInDegrees(reverseLimitDegrees); + talon.setReverseSoftLimit(rawPosition); + } return this; } @Override - public TalonSRX enableReverseSoftLimit(boolean enable) { + public HardwareTalonSRX enableReverseSoftLimit(boolean enable) { talon.enableReverseSoftLimit(enable); return this; }