diff --git a/strongback-testing/src/org/strongback/mock/Mock.java b/strongback-testing/src/org/strongback/mock/Mock.java index 8c4f935..233421c 100644 --- a/strongback-testing/src/org/strongback/mock/Mock.java +++ b/strongback-testing/src/org/strongback/mock/Mock.java @@ -17,7 +17,6 @@ package org.strongback.mock; import java.util.concurrent.atomic.AtomicLong; - import org.strongback.components.Fuse; import org.strongback.components.TalonSRX; import org.strongback.control.Controller; @@ -96,6 +95,15 @@ public static MockAngleSensor angleSensor() { return new MockAngleSensor(); } + /** + * Create a mock revolution sensor. + * + * @return the mock rev sensor, never null + */ + public static MockRevSensor revSensor() { + return new MockRevSensor(); + } + /** * Create a mock compass. * diff --git a/strongback-testing/src/org/strongback/mock/MockRevSensor.java b/strongback-testing/src/org/strongback/mock/MockRevSensor.java new file mode 100644 index 0000000..e42bb65 --- /dev/null +++ b/strongback-testing/src/org/strongback/mock/MockRevSensor.java @@ -0,0 +1,35 @@ +package org.strongback.mock; + +import org.strongback.components.RevSensor; + +public class MockRevSensor implements RevSensor { + private double revolutions; + private double revolutionsPerMinute; + + protected MockRevSensor() { + this(0.0, 0.0); + } + + protected MockRevSensor(double revolutions, double revolutionsPerMinute) { + this.revolutions = revolutions; + this.revolutionsPerMinute = revolutionsPerMinute; + } + + public void setRevolutions(double revolutions) { + this.revolutions = revolutions; + } + + public void setRevolutionsPerMinute(double revolutionsPerMinute) { + this.revolutionsPerMinute = revolutionsPerMinute; + } + + @Override + public double getRevolutions() { + return revolutions; + } + + @Override + public double getRevolutionsPerMinute() { + return revolutionsPerMinute; + } +} diff --git a/strongback-testing/src/org/strongback/mock/MockTalonSRX.java b/strongback-testing/src/org/strongback/mock/MockTalonSRX.java index aa8beb7..bb5cb4d 100644 --- a/strongback-testing/src/org/strongback/mock/MockTalonSRX.java +++ b/strongback-testing/src/org/strongback/mock/MockTalonSRX.java @@ -17,7 +17,7 @@ package org.strongback.mock; import org.strongback.components.Fuse; -import org.strongback.components.Gyroscope; +import org.strongback.components.RevSensor; import org.strongback.components.Switch; import org.strongback.components.TalonSRX; import org.strongback.components.TemperatureSensor; @@ -25,12 +25,13 @@ public class MockTalonSRX extends MockMotor implements TalonSRX { - private static final Gyroscope NO_OP_GYRO = new MockGyroscope(); + private static final RevSensor NO_OP_REV = new MockRevSensor(); private final int deviceId; - private final MockGyroscope encoderInput = new MockGyroscope(); - private final MockGyroscope analogInput = new MockGyroscope(); - private Gyroscope selectedInput = NO_OP_GYRO; + private final MockRevSensor encoderInput = new MockRevSensor(); + private final MockRevSensor analogInput = new MockRevSensor(); + private final MockRevSensor pwmInput = new MockRevSensor(); + private RevSensor selectedInput = NO_OP_REV; private final MockCurrentSensor current = new MockCurrentSensor(); private final MockVoltageSensor voltage = new MockVoltageSensor(); private final MockVoltageSensor busVoltage = new MockVoltageSensor(); @@ -60,17 +61,22 @@ public MockTalonSRX setSpeed(double speed) { } @Override - public MockGyroscope getAnalogInput() { + public MockRevSensor getAnalogInput() { return analogInput; } @Override - public MockGyroscope getEncoderInput() { + public RevSensor getPwmInput() { + return pwmInput; + } + + @Override + public MockRevSensor getEncoderInput() { return encoderInput; } @Override - public Gyroscope getSelectedSensor() { + public RevSensor getSelectedSensor() { return selectedInput; } @@ -89,9 +95,11 @@ public MockTalonSRX setFeedbackDevice(FeedbackDevice device) { case QUADRATURE_ENCODER: this.selectedInput = encoderInput; break; + case PULSE_WIDTH: + this.selectedInput = pwmInput; case ENCODER_FALLING: case ENCODER_RISING: - selectedInput = NO_OP_GYRO; + selectedInput = NO_OP_REV; break; } return this; @@ -127,6 +135,16 @@ public TemperatureSensor getTemperatureSensor() { return temperature; } + @Override + public void setEncoderCodesPerRevolution(int codesPerRev) { + + } + + @Override + public void setPotentiometerTurns(int turns) { + + } + @Override public MockSwitch getForwardLimitSwitch() { return forwardLimitSwitch; diff --git a/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_AnalogInputSensorTest.java b/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_AnalogInputSensorTest.java deleted file mode 100644 index da4db3c..0000000 --- a/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_AnalogInputSensorTest.java +++ /dev/null @@ -1,119 +0,0 @@ -/* - * Strongback - * Copyright 2015, Strongback and individual contributors by the @authors tag. - * See the COPYRIGHT.txt in the distribution for a full listing of individual - * contributors. - * - * Licensed under the MIT License; you may not use this file except in - * compliance with the License. You may obtain a copy of the License at - * http://opensource.org/licenses/MIT - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package org.strongback.hardware; - -import static org.fest.assertions.Assertions.assertThat; - -import org.fest.assertions.Delta; -import org.junit.Before; -import org.junit.Test; -import org.strongback.hardware.HardwareTalonSRX.AnalogInputSensor; - -public class HardwareTalonSRX_AnalogInputSensorTest { - - private static final Delta DELTA = Delta.delta(1); - - private double analogPosition = 0.0; - private double analogVelocity = 0.0; // changes in volts per cycle - private double analogRange = 1023; // 10 bit values - private double analogTurnsOverVoltageRange = 1; - private double analogVoltageRange = 3.3; - private double cyclePeriodInSeconds = 0.1; - private AnalogInputSensor sensor; - - @Before - public void beforeEach() { - } - - protected AnalogInputSensor createSensor() { - sensor = new AnalogInputSensor(() -> analogPosition, - () -> analogVelocity, - analogRange, - analogTurnsOverVoltageRange / analogVoltageRange, - analogVoltageRange, - () -> cyclePeriodInSeconds); - return sensor; - } - - @Test - public void shouldReturnZeroWhenMeasuredPositionAndVelocityAreZero() { - analogPosition = 0; - analogTurnsOverVoltageRange = 1; - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(0.0d, DELTA); - assertThat(sensor.getHeading()).isEqualTo(0.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); - assertThat(sensor.rawPositionForAngleInDegrees(0.0d)).isEqualTo(analogPosition, DELTA); - } - - @Test - public void shouldReturnZeroWhenMeasuredPositionIsHalfRotationAndVelocityAreZero() { - analogPosition = 1024/2 - 1; - analogTurnsOverVoltageRange = 1; - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(180.0d, DELTA); - assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); - assertThat(sensor.rawPositionForAngleInDegrees(180.0d)).isEqualTo(analogPosition, DELTA); - } - - @Test - public void shouldReturnZeroWhenMeasuredPositionIsOneRotationAndVelocityAreZero() { - analogPosition = 1024-1; - analogTurnsOverVoltageRange = 1; - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(360.0d, DELTA); - assertThat(sensor.getHeading()).isEqualTo(0.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); - assertThat(sensor.rawPositionForAngleInDegrees(360.0d)).isEqualTo(analogPosition, DELTA); - } - - @Test - public void shouldReturnZeroWhenMeasuredPositionIsOneAndOneHalfRotationAndVelocityAreZero() { - analogPosition = (int)(1024 * 1.5) - 1; - analogTurnsOverVoltageRange = 1; - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(540.0d, DELTA); - assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); - assertThat(sensor.rawPositionForAngleInDegrees(540.0d)).isEqualTo(analogPosition, DELTA); - } - - @Test - public void shouldReturnZeroWhenMeasuredPositionIsThreeAndOneHalfRotationAndVelocityAreZero() { - analogPosition = (int)(1024 * 3.5) - 1; - analogTurnsOverVoltageRange = 1; - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(1260.0d, DELTA); - assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); - assertThat(sensor.rawPositionForAngleInDegrees(1260.0d)).isEqualTo(analogPosition, Delta.delta(5.0)); - } - - @Test - public void shouldHandleMeasuredPositionAndVelocity() { - analogPosition = (int)(1024 * 3.5) - 1; - analogVelocity = 1023; // starts from 0 and does full rotation - analogTurnsOverVoltageRange = 1; - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(1260.0d, DELTA); - assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(360.0 / cyclePeriodInSeconds, DELTA); // degrees per second - assertThat(sensor.rawPositionForAngleInDegrees(1260.0d)).isEqualTo(analogPosition, Delta.delta(5.0)); - } - -} diff --git a/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_EncoderInputSensorTest.java b/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_EncoderInputSensorTest.java deleted file mode 100644 index 6423bb3..0000000 --- a/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_EncoderInputSensorTest.java +++ /dev/null @@ -1,126 +0,0 @@ -/* - * Strongback - * Copyright 2015, Strongback and individual contributors by the @authors tag. - * See the COPYRIGHT.txt in the distribution for a full listing of individual - * contributors. - * - * Licensed under the MIT License; you may not use this file except in - * compliance with the License. You may obtain a copy of the License at - * http://opensource.org/licenses/MIT - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package org.strongback.hardware; - -import static org.fest.assertions.Assertions.assertThat; - -import org.fest.assertions.Delta; -import org.junit.Before; -import org.junit.Test; -import org.strongback.hardware.HardwareTalonSRX.EncoderInputSensor; - -public class HardwareTalonSRX_EncoderInputSensorTest { - - private static final Delta DELTA = Delta.delta(0.00001); - - private double positionInEdges = 0.0; - private double velocityInEdges = 0.0; - private double cyclePeriodInSeconds = 0.1; - private double rotationInDegrees = 0.0; - private int encoderPulsesPerRotation = 360; - private int encoderEdgesPerPulse = 4; // 4x mode, or 4 rising edges per pulse - private EncoderInputSensor sensor; - - @Before - public void beforeEach() { - cyclePeriodInSeconds = 0.1; - } - - protected EncoderInputSensor createSensor() { - sensor = new EncoderInputSensor(() -> positionInEdges, () -> velocityInEdges, encoderPulsesPerRotation / 360.0, // pulses - // per - // degree - encoderEdgesPerPulse, () -> cyclePeriodInSeconds); - return sensor; - } - - @Test - public void shouldReturnZeroWhenMeasuredPositionAndVelocityAreZero() { - encoderPulsesPerRotation = 360; - positionInEdges = 0.0; - velocityInEdges = 0.0; - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(0.0d, DELTA); - assertThat(sensor.getHeading()).isEqualTo(0.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); - assertThat(sensor.rawPositionForAngleInDegrees(0.0d)).isEqualTo(positionInEdges, DELTA); - } - - @Test - public void shouldReturnZeroWhenMeasuredPositionIsHalfRotationAndVelocityAreZero() { - encoderPulsesPerRotation = 360; - positionInEdges = 180 * 4; // edges for 180 degrees - velocityInEdges = 0.0; - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(180.0d, DELTA); - assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); - assertThat(sensor.rawPositionForAngleInDegrees(180.0d)).isEqualTo(positionInEdges, DELTA); - } - - @Test - public void shouldReturnZeroWhenMeasuredPositionIsOneAndOneHalfRotationAndVelocityAreZero() { - encoderPulsesPerRotation = 360; - positionInEdges = (180+360) * 4; // edges for 540 degrees - velocityInEdges = 0.0; - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(540.0d, DELTA); - assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); - assertThat(sensor.rawPositionForAngleInDegrees(540.0d)).isEqualTo(positionInEdges, DELTA); - } - - @Test - public void shouldReturnZeroWhenMeasuredPositionIsThreeAndOneHalfRotationAndVelocityAreZero() { - encoderPulsesPerRotation = 360; - positionInEdges = (180+360*3) * 4; // edges for 1260 degrees - velocityInEdges = 0.0; - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(1260.0d, DELTA); - assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); - assertThat(sensor.rawPositionForAngleInDegrees(1260.0d)).isEqualTo(positionInEdges, DELTA); - } - - @Test - public void shouldReturnZeroWhenMeasuredPositionIsThreeAndOneHalfRotationAndVelocityAreZeroWithPrecisionEncoder() { - encoderPulsesPerRotation = 250; - rotationInDegrees = 180+360*3; - positionInEdges = rotationInDegrees * encoderPulsesPerRotation /360 * 4; - velocityInEdges = 0.0; - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(1260.0d, DELTA); - assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); - assertThat(sensor.rawPositionForAngleInDegrees(1260.0d)).isEqualTo(positionInEdges, DELTA); - } - - @Test - public void shouldHandleMeasuredPositionOfOneAndVelocityOfTwenty() { - double rotationsPerSecond = 20; - encoderPulsesPerRotation = 360; - rotationInDegrees = 360; - positionInEdges = rotationInDegrees * encoderPulsesPerRotation /360 * 4; - velocityInEdges = rotationsPerSecond * 4 * 360.0 * cyclePeriodInSeconds; // per cycle, or 28800 position units / second - createSensor(); - assertThat(sensor.getAngle()).isEqualTo(360.0, DELTA); - assertThat(sensor.getHeading()).isEqualTo(0.0d, DELTA); - assertThat(sensor.getRate()).isEqualTo(rotationsPerSecond * 360.0, DELTA); // degrees per second - assertThat(sensor.rawPositionForAngleInDegrees(360.0d)).isEqualTo(positionInEdges, DELTA); - } - -} diff --git a/strongback/src/org/strongback/components/RevSensor.java b/strongback/src/org/strongback/components/RevSensor.java new file mode 100644 index 0000000..d1bf9b5 --- /dev/null +++ b/strongback/src/org/strongback/components/RevSensor.java @@ -0,0 +1,70 @@ +package org.strongback.components; + + +import java.util.function.DoubleSupplier; + +/** + * An angular sensor designed for use with motors. It is able to return position as a number of revolutions, and speed + * as a number of revolutions per minute (RPM). It provides an implementation of {@link Gyroscope} as a convenience, + * particularly because this interface is being used to replace Gyroscope in some places. + * + * @author Adam Gausmann + */ +public interface RevSensor extends Gyroscope { + + /** + * Returns the position of the sensor as a number of revolutions. + * + * @return The angular position in revolutions. + */ + double getRevolutions(); + + /** + * Returns the speed that the sensor rotates. + * + * @return The angular speed as revolutions per minute (RPM). + */ + double getRevolutionsPerMinute(); + + /** + * Computes the angle in degrees by taking the number of revolutions and multiplying by 360 degrees per revolution. + * + * @return The angular position of this sensor in degrees. + */ + @Override + default double getAngle() { + return getRevolutions() * 360.0; + } + + /** + * Computes the rate in degrees per second by taking the RPM, multiplying by 360 degrees per revolution, and + * dividing by 60 seconds per minute. + * + * @return The rotation rate of this sensor in degrees per minute. + */ + @Override + default double getRate() { + return getRevolutionsPerMinute() * 360.0 / 60.0; + } + + /** + * Creates a RevSensor based upon two functions that return revolutions and RPM. + * + * @param revSupplier The function that returns the number of revolutions. + * @param rpmSupplier The function that returns the RPM. + * @return A new RevSensor implemented using the two functions. + */ + static RevSensor create(DoubleSupplier revSupplier, DoubleSupplier rpmSupplier) { + return new RevSensor() { + @Override + public double getRevolutions() { + return revSupplier.getAsDouble(); + } + + @Override + public double getRevolutionsPerMinute() { + return rpmSupplier.getAsDouble(); + } + }; + } +} diff --git a/strongback/src/org/strongback/components/TalonSRX.java b/strongback/src/org/strongback/components/TalonSRX.java index 615ac22..c9bf817 100644 --- a/strongback/src/org/strongback/components/TalonSRX.java +++ b/strongback/src/org/strongback/components/TalonSRX.java @@ -16,6 +16,7 @@ package org.strongback.components; +import com.ctre.CANTalon; import org.strongback.annotation.Experimental; /** @@ -54,32 +55,34 @@ public default AngleSensor getAngleSensor() { } /** - * Get the current encoder angle and rate, regardless of whether it is the current feedback device. + * Get the current encoder position and rate, regardless of whether it is the current feedback device. * * @return the gyroscope that reads the encoder sensor; or null if the motor was created with no quadrature encoder input * @see org.strongback.hardware.Hardware.Motors#talonSRX(int) * @see org.strongback.hardware.Hardware.Motors#talonSRX(int, double) * @see org.strongback.hardware.Hardware.Motors#talonSRX(int, double, double) */ - public Gyroscope getEncoderInput(); + public RevSensor getEncoderInput(); /** - * Get the current analog angle and rate, regardless of whether it is the current feedback device. + * Get the current analog position and rate, regardless of whether it is the current feedback device. * * @return the gyroscope that reads the 3.3V analog sensor; or null if the motor was created with no analog input * @see org.strongback.hardware.Hardware.Motors#talonSRX(int) * @see org.strongback.hardware.Hardware.Motors#talonSRX(int, double) * @see org.strongback.hardware.Hardware.Motors#talonSRX(int, double, double) */ - public Gyroscope getAnalogInput(); + public RevSensor getAnalogInput(); + + public RevSensor getPwmInput(); /** - * Get the input angle and rate of the current {@link #setFeedbackDevice(FeedbackDevice) feedback device}. + * Get the input position and rate of the current {@link #setFeedbackDevice(FeedbackDevice) feedback device}. * * @return the selected input device sensor; never null, but it may return a meaningless value if a sensor is not physically * wired as an input to the Talon device */ - public Gyroscope getSelectedSensor(); + public RevSensor getSelectedSensor(); /** * Get the Talon SRX's output current sensor. @@ -109,6 +112,10 @@ public default AngleSensor getAngleSensor() { */ public TemperatureSensor getTemperatureSensor(); + void setEncoderCodesPerRevolution(int codesPerRev); + + void setPotentiometerTurns(int turns); + /** * Set the feedback device for this controller. * @@ -140,7 +147,7 @@ public default AngleSensor getAngleSensor() { public TalonSRX reverseSensor(boolean flip); /** - * Set the soft limit for the forward throttle, in terms of the angle as measured by the {@link #getSelectedSensor() + * Set the soft limit for the forward throttle, in terms of the position 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)}. @@ -150,15 +157,15 @@ public default AngleSensor getAngleSensor() { * 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 forwardLimitInDegrees the angle at which the forward throttle should be disabled, where the angle in terms of the + * @param forwardLimit the position at which the forward throttle should be disabled, in terms of the * {@link #getSelectedSensor()} * @return this object so that methods can be chained; never null * @see #enableForwardSoftLimit(boolean) */ - public TalonSRX setForwardSoftLimit(int forwardLimitInDegrees); + public TalonSRX setForwardSoftLimit(int forwardLimit); /** - * Set the soft limit for the reverse throttle, in terms of the angle as measured by the {@link #getSelectedSensor() + * Set the soft limit for the reverse throttle, in terms of the position 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)}. @@ -168,12 +175,12 @@ public default AngleSensor getAngleSensor() { * 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 reverseLimitInDegrees the angle at which the reverse throttle should be disabled, where the angle in terms of the + * @param reverseLimit the position at which the reverse throttle should be disabled, in terms of the * {@link #getSelectedSensor()} * @return this object so that methods can be chained; never null * @see #enableForwardSoftLimit(boolean) */ - public TalonSRX setReverseSoftLimit(int reverseLimitInDegrees); + public TalonSRX setReverseSoftLimit(int reverseLimit); /** * Enable the soft limit for forward throttle, which is set via the {@link #setForwardSoftLimit(int)}. @@ -326,22 +333,22 @@ public enum FeedbackDevice { /** * Use Quadrature Encoder. */ - QUADRATURE_ENCODER(0), + QUADRATURE_ENCODER(CANTalon.FeedbackDevice.QuadEncoder.value), /** * Analog potentiometer or any other analog device, 0-3.3V */ - ANALOG_POTENTIOMETER(2), + ANALOG_POTENTIOMETER(CANTalon.FeedbackDevice.AnalogPot.value), /** * Analog encoder or any other analog device, 0-3.3V */ - ANALOG_ENCODER(3), + ANALOG_ENCODER(CANTalon.FeedbackDevice.AnalogEncoder.value), /** * Encoder that increments position per rising edge (and never decrements) on Quadrature-A. */ - ENCODER_RISING(4), + ENCODER_RISING(CANTalon.FeedbackDevice.EncRising.value), /** * Encoder that increments position per falling edge (and never decrements) on Quadrature-A. Note: Was not supported in @@ -349,22 +356,22 @@ public enum FeedbackDevice { * Talon SRX Software * Reference Manual, section 21.3 */ - ENCODER_FALLING(5), + ENCODER_FALLING(CANTalon.FeedbackDevice.EncFalling.value), /** * CTRE magnetic encoder with relative position */ - MAGNETIC_ENCODER_RELATIVE(6), + MAGNETIC_ENCODER_RELATIVE(CANTalon.FeedbackDevice.CtreMagEncoder_Relative.value), /** * CTRE magnetic encoder with absolute position */ - MAGNETIC_ENCODER_ABSOLUTE(7), + MAGNETIC_ENCODER_ABSOLUTE(CANTalon.FeedbackDevice.CtreMagEncoder_Absolute.value), /** * Pulse width */ - PULSE_WIDTH(8); + PULSE_WIDTH(CANTalon.FeedbackDevice.PulseWidth.value); public int value; diff --git a/strongback/src/org/strongback/control/TalonController.java b/strongback/src/org/strongback/control/TalonController.java index 1c47b4d..198571d 100644 --- a/strongback/src/org/strongback/control/TalonController.java +++ b/strongback/src/org/strongback/control/TalonController.java @@ -179,23 +179,25 @@ public int value() { public TalonController setFeedbackDevice(FeedbackDevice device); /** - * Get the current target angle for this controller as defined by the selected input sensor in degrees. - * @return the target angle in degrees as defined by the selected input sensor + * Get the current target for this controller as previously set by {@link #withTarget(double)}. + * @return The controller's target. For position and speed control modes, the target will be in revolutions or + * revolutions per minute. For voltage and current control modes, the target will be in volts or amperes. * @see #withTarget(double) */ @Override public double getTarget(); /** - * Sets the target angle for the controller's selected input sensor in degrees. + * Sets the target speed, position, voltage, or current for the controller. * - * @param angleInDegrees the desired angle in degrees that this controller will use as a target as defined by the selected - * input sensor + * @param target the desired value that the closed-loop controller will use as its target. For position and speed + * control modes, the target should be in revolutions or revolutions per minute. For voltage and + * current control modes, the target should be in volts or amperes. * @return this object so that methods can be chained; never null * @see #getTarget() */ @Override - public TalonController withTarget(double angleInDegrees); + public TalonController withTarget(double target); @Override public TalonController setStatusFrameRate(StatusFrameRate frameRate, int periodMillis); diff --git a/strongback/src/org/strongback/hardware/Hardware.java b/strongback/src/org/strongback/hardware/Hardware.java index 4a286cf..84ce823 100644 --- a/strongback/src/org/strongback/hardware/Hardware.java +++ b/strongback/src/org/strongback/hardware/Hardware.java @@ -16,30 +16,8 @@ package org.strongback.hardware; -import org.strongback.annotation.Experimental; -import org.strongback.components.Accelerometer; -import org.strongback.components.AngleSensor; -import org.strongback.components.DistanceSensor; -import org.strongback.components.Gyroscope; -import org.strongback.components.Motor; -import org.strongback.components.PneumaticsModule; -import org.strongback.components.PowerPanel; -import org.strongback.components.Relay; -import org.strongback.components.Solenoid; -import org.strongback.components.Switch; -import org.strongback.components.TalonSRX; -import org.strongback.components.ThreeAxisAccelerometer; -import org.strongback.components.TwoAxisAccelerometer; -import org.strongback.components.ui.FlightStick; -import org.strongback.components.ui.Gamepad; -import org.strongback.components.ui.InputDevice; -import org.strongback.control.TalonController; -import org.strongback.function.DoubleToDoubleFunction; -import org.strongback.util.Values; - import com.ctre.CANTalon; import com.ctre.CANTalon.TalonControlMode; - import edu.wpi.first.wpilibj.ADXL345_I2C; import edu.wpi.first.wpilibj.ADXL345_SPI; import edu.wpi.first.wpilibj.ADXRS450_Gyro; @@ -65,6 +43,26 @@ import edu.wpi.first.wpilibj.VictorSP; import edu.wpi.first.wpilibj.interfaces.Accelerometer.Range; import edu.wpi.first.wpilibj.interfaces.Gyro; +import org.strongback.annotation.Experimental; +import org.strongback.components.Accelerometer; +import org.strongback.components.AngleSensor; +import org.strongback.components.DistanceSensor; +import org.strongback.components.Gyroscope; +import org.strongback.components.Motor; +import org.strongback.components.PneumaticsModule; +import org.strongback.components.PowerPanel; +import org.strongback.components.Relay; +import org.strongback.components.Solenoid; +import org.strongback.components.Switch; +import org.strongback.components.TalonSRX; +import org.strongback.components.ThreeAxisAccelerometer; +import org.strongback.components.TwoAxisAccelerometer; +import org.strongback.components.ui.FlightStick; +import org.strongback.components.ui.Gamepad; +import org.strongback.components.ui.InputDevice; +import org.strongback.control.TalonController; +import org.strongback.function.DoubleToDoubleFunction; +import org.strongback.util.Values; /** * The factory methods that will create component implementations corresponding to physical hardware on the robot. Nested @@ -672,7 +670,7 @@ public static TalonSRX talonSRX(CANTalon talon, double pulsesPerDegree) { @Experimental public static TalonSRX talonSRX(CANTalon talon, double pulsesPerDegree, double analogTurnsOverVoltageRange) { if (talon == null) throw new IllegalArgumentException("The CANTalon reference may not be null"); - return new HardwareTalonSRX(talon, pulsesPerDegree, analogTurnsOverVoltageRange); + return new HardwareTalonSRX(talon); } /** @@ -690,7 +688,7 @@ public static TalonSRX talonSRX(int deviceNumber, TalonSRX leader, boolean rever talon.changeControlMode(TalonControlMode.Follower); talon.set(leader.getDeviceID()); talon.reverseOutput(reverse); - return talonSRX(talon, 0.0d, 0.0d); + return talonSRX(talon); } /** @@ -737,10 +735,9 @@ public static final class Controllers { * @return the interface for managing and using the Talon SRX hardware-based PID controller; never null */ @Experimental - public static TalonController talonController(int deviceNumber, double pulsesPerDegree, - double analogTurnsOverVoltageRange) { + public static TalonController talonController(int deviceNumber) { CANTalon talon = new CANTalon(deviceNumber); - HardwareTalonController c = new HardwareTalonController(talon, pulsesPerDegree, analogTurnsOverVoltageRange); + HardwareTalonController c = new HardwareTalonController(talon); return c; } @@ -759,9 +756,8 @@ public static TalonController talonController(int deviceNumber, double pulsesPer * @return the interface for managing and using the Talon SRX hardware-based PID controller; never null */ @Experimental - public static TalonController talonController(CANTalon talon, double pulsesPerDegree, - double analogTurnsOverVoltageRange) { - return new HardwareTalonController(talon, pulsesPerDegree, analogTurnsOverVoltageRange); + public static TalonController talonController(CANTalon talon) { + return new HardwareTalonController(talon); } } diff --git a/strongback/src/org/strongback/hardware/HardwareTalonController.java b/strongback/src/org/strongback/hardware/HardwareTalonController.java index 5f85a4e..b46f61c 100644 --- a/strongback/src/org/strongback/hardware/HardwareTalonController.java +++ b/strongback/src/org/strongback/hardware/HardwareTalonController.java @@ -16,19 +16,17 @@ package org.strongback.hardware; +import com.ctre.CANTalon; +import com.ctre.CANTalon.TalonControlMode; import java.util.Collections; import java.util.Set; import java.util.concurrent.ConcurrentHashMap; - import org.strongback.Executable; import org.strongback.annotation.ThreadSafe; import org.strongback.control.Controller; import org.strongback.control.PIDController; import org.strongback.control.TalonController; -import com.ctre.CANTalon; -import com.ctre.CANTalon.TalonControlMode; - /** * A hardware-based Talon SRX PID controller. */ @@ -88,8 +86,8 @@ public double getCloseLoopRampRate() { private volatile int currentProfile = 0; private final Set profiles = Collections.newSetFromMap(new ConcurrentHashMap<>()); - HardwareTalonController(CANTalon talon, double pulsesPerDegree, double analogTurnsOverVoltageRange) { - super(talon, pulsesPerDegree, analogTurnsOverVoltageRange); + HardwareTalonController(CANTalon talon) { + super(talon); profiles.add(currentProfile); } @@ -117,13 +115,12 @@ public double getValue() { @Override public double getTarget() { - double targetPosition = talon.getSetpoint(); - return this.selectedInput.angleInDegreesFromRawPosition(targetPosition); + return talon.getSetpoint(); } @Override - public TalonController withTarget(double angleInDegrees) { - talon.set(this.selectedInput.rawPositionForAngleInDegrees(angleInDegrees)); + public TalonController withTarget(double target) { + talon.setSetpoint(target); return this; } diff --git a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java index cfe9ec7..f06a692 100644 --- a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java +++ b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java @@ -16,20 +16,16 @@ package org.strongback.hardware; -import java.util.function.DoubleSupplier; - -import org.strongback.Strongback; +import com.ctre.CANTalon; +import com.ctre.CANTalon.TalonControlMode; import org.strongback.annotation.Immutable; import org.strongback.components.CurrentSensor; -import org.strongback.components.Gyroscope; +import org.strongback.components.RevSensor; import org.strongback.components.Switch; import org.strongback.components.TalonSRX; import org.strongback.components.TemperatureSensor; import org.strongback.components.VoltageSensor; -import com.ctre.CANTalon; -import com.ctre.CANTalon.TalonControlMode; - /** * Talon speed controller with position and current sensor * @@ -39,164 +35,19 @@ */ @Immutable class HardwareTalonSRX implements TalonSRX { - - protected static interface InputSensor extends Gyroscope { - public double rawPositionForAngleInDegrees( double angle ); - public double angleInDegreesFromRawPosition( double position ); - } - - protected static final InputSensor NO_OP_SENSOR = new InputSensor() { - - @Override - public double getAngle() { - return 0; - } - - @Override - public double getRate() { - return 0; - } - @Override - public InputSensor zero() { - return this; // do nothing - } - - @Override - public double rawPositionForAngleInDegrees(double angleInDegrees) { - return 0.0; - } - - @Override - public double angleInDegreesFromRawPosition(double position) { - return 0.0; - } - }; - - protected static final class EncoderInputSensor implements InputSensor { - private double zero = 0.0; - private final DoubleSupplier positionInEdges; - private final DoubleSupplier velocityInEdgesPerCycle; - private final DoubleSupplier cyclePeriodInSeconds; - private final double pulsesPerDegree; - private final double edgesPerPulse; - - protected EncoderInputSensor( DoubleSupplier positionInEdges, DoubleSupplier velocityInEdgesPerCycle, - double pulsesPerDegree, int edgesPerPulse, DoubleSupplier cyclesPeriodInSeconds ) { - this.positionInEdges = positionInEdges; - this.velocityInEdgesPerCycle = velocityInEdgesPerCycle; - this.cyclePeriodInSeconds = cyclesPeriodInSeconds; - this.pulsesPerDegree = pulsesPerDegree; - this.edgesPerPulse = edgesPerPulse; - } - - @Override - public double rawPositionForAngleInDegrees(double angle) { - // Units: (degrees) x (pulses/degrees) * (edges/pulses) = (edges) - double relativeInput = angle * pulsesPerDegree * edgesPerPulse; - return relativeInput + zero; - } - - @Override - public double angleInDegreesFromRawPosition(double position) { - // Units: (edges) x (pulse/edges) x (degrees/pulse) = degrees - return (position - zero) / edgesPerPulse / pulsesPerDegree; - } - - @Override - public double getAngle() { - return angleInDegreesFromRawPosition(positionInEdges.getAsDouble()); - } - - @Override - public double getRate() { - // Units: (edges/cycle) * (pulses/edge) x (degrees/pulse) x (cycles/second) = (degrees/second) - return velocityInEdgesPerCycle.getAsDouble() / edgesPerPulse / pulsesPerDegree / cyclePeriodInSeconds.getAsDouble(); - } - - @Override - public Gyroscope zero() { - zero = positionInEdges.getAsDouble(); - return this; - } - } - - protected static final class AnalogInputSensor implements InputSensor { - private double zero = 0.0; - private final DoubleSupplier analogPosition; - private final DoubleSupplier changeInVoltsPerCycle; - private final DoubleSupplier cyclePeriodInSeconds; - private final double analogRange; - private final double analogTurnsPerVolt; - private final double voltageRange; - - protected AnalogInputSensor( DoubleSupplier analogPosition, DoubleSupplier changeInVoltsPerCycle, - double analogRange, double analogTurnsPerVolt, double voltageRange, DoubleSupplier cyclePeriodInSeconds ) { - this.analogPosition = analogPosition; - this.changeInVoltsPerCycle = changeInVoltsPerCycle; - this.cyclePeriodInSeconds = cyclePeriodInSeconds; - this.analogRange = analogRange; - this.analogTurnsPerVolt = analogTurnsPerVolt; - this.voltageRange = voltageRange; - } - - @Override - public double rawPositionForAngleInDegrees(double angle) { - // Units: (0-1023) / 1023 x (turns/volt) x (volts) x (degrees/turn) = degrees - // Units: (degrees) x (turns/degrees) x (1/volts) x (volts/turn) * 1023 = (0-1023) - double relativeInput = angle / 360.0 / voltageRange / analogTurnsPerVolt * analogRange; - return relativeInput + zero; - } - - @Override - public double angleInDegreesFromRawPosition(double position) { - // Units: (0-1023) / 1023 x (turns/volt) x (volts) x (degrees/turn) = degrees - return (position - zero) / analogRange * analogTurnsPerVolt * voltageRange * 360.0; - } - @Override - public double getAngle() { - return angleInDegreesFromRawPosition(analogPosition.getAsDouble()); - } - - @Override - public double getRate() { - // Units: (0-1023)/cycle / 1023 x (turns/volt) x (volts) x (degrees/turn) x (cycles/second) = (degrees/second) - return changeInVoltsPerCycle.getAsDouble() / analogRange * analogTurnsPerVolt * voltageRange * 360.0 - / cyclePeriodInSeconds.getAsDouble(); - } - - @Override - public Gyroscope zero() { - zero = analogPosition.getAsDouble(); - return this; - } - } - - protected static EncoderInputSensor encoderInput(DoubleSupplier positionInPulses, DoubleSupplier velocityInPulsesPerCycle, - double pulsesPerDegree, int risesPerPulse, DoubleSupplier cyclePeriodInSeconds) { - if ( pulsesPerDegree <= 0.0000001d && pulsesPerDegree >= 0.0000001d ) return null; - return new EncoderInputSensor(positionInPulses, velocityInPulsesPerCycle, pulsesPerDegree, risesPerPulse, cyclePeriodInSeconds); - } - - protected static AnalogInputSensor analogInput(DoubleSupplier analogPosition, DoubleSupplier changeInVoltsPerCycle, - double analogRange, double analogTurnsPerVolt, double voltageRange, DoubleSupplier cyclesPeriodInSeconds) { - if ( analogTurnsPerVolt <= 0.0000001d && analogTurnsPerVolt >= 0.0000001d ) return null; - return new AnalogInputSensor(analogPosition, changeInVoltsPerCycle, analogRange, analogTurnsPerVolt, voltageRange, cyclesPeriodInSeconds); - } - private static final double DEFAULT_ANALOG_RATE = 0.100; private static final double DEFAULT_QUADRATURE_RATE = 0.100; private static final double DEFAULT_FEEDBACK_RATE = 0.020; private static final int RISES_PER_PULSE = 4; // 4x mode private static final double MAX_ANALOG_VOLTAGE = 3.3; // 0-3.3V - private static final double MAX_ANALOG_RANGE = 1023; // 10 bits non-continuous + private static final RevSensor NO_OP_SENSOR = RevSensor.create(() -> 0.0, () -> 0.0); protected final CANTalon talon; - protected final InputSensor encoderInput; - protected final InputSensor analogInput; - protected final InputSensor selectedEncoderInput; - protected final InputSensor selectedAnalogInput; - protected volatile InputSensor selectedInput = NO_OP_SENSOR; + protected final RevSensor encoderInput; + protected final RevSensor analogInput; + protected final RevSensor pwmInput; + protected final RevSensor selectedInput; protected volatile double quadratureRateInSeconds = DEFAULT_QUADRATURE_RATE; protected volatile double analogRateInSeconds = DEFAULT_ANALOG_RATE; protected volatile double feedbackRateInSeconds = DEFAULT_FEEDBACK_RATE; @@ -209,7 +60,7 @@ protected static AnalogInputSensor analogInput(DoubleSupplier analogPosition, Do protected final Faults instantaneousFaults; protected final Faults stickyFaults; - HardwareTalonSRX(CANTalon talon, double pulsesPerDegree, double analogTurnsOverVoltageRange) { + HardwareTalonSRX(CANTalon talon) { this.talon = talon; this.forwardLimitSwitch = talon::isFwdLimitSwitchClosed; @@ -218,28 +69,10 @@ protected static AnalogInputSensor analogInput(DoubleSupplier analogPosition, Do this.outputVoltage = talon::getOutputVoltage; this.busVoltage = talon::getBusVoltage; this.temperature = talon::getTemperature; - this.encoderInput = encoderInput(talon::getEncPosition, - talon::getEncVelocity, - pulsesPerDegree, - RISES_PER_PULSE, - () -> quadratureRateInSeconds); - this.analogInput = analogInput(talon::getAnalogInPosition, - talon::getAnalogInVelocity, - MAX_ANALOG_RANGE, - analogTurnsOverVoltageRange / MAX_ANALOG_VOLTAGE, - MAX_ANALOG_VOLTAGE, - () -> analogRateInSeconds); - this.selectedEncoderInput = encoderInput(talon::getPosition, - talon::getSpeed, - pulsesPerDegree, - RISES_PER_PULSE, - () -> feedbackRateInSeconds); - this.selectedAnalogInput = analogInput(talon::getPosition, - talon::getSpeed, - MAX_ANALOG_RANGE, - analogTurnsOverVoltageRange / MAX_ANALOG_VOLTAGE, - MAX_ANALOG_VOLTAGE, - () -> feedbackRateInSeconds); + this.encoderInput = RevSensor.create(talon::getEncPosition, talon::getEncVelocity); + this.analogInput = RevSensor.create(talon::getAnalogInPosition, talon::getAnalogInVelocity); + this.pwmInput = RevSensor.create(talon::getPulseWidthPosition, talon::getPulseWidthVelocity); + this.selectedInput = RevSensor.create(talon::getPosition, talon::getSpeed); this.instantaneousFaults = new Faults() { @Override public Switch forwardLimitSwitch() { @@ -339,56 +172,38 @@ public void stop() { } @Override - public Gyroscope getEncoderInput() { + public RevSensor getEncoderInput() { return encoderInput; } @Override - public Gyroscope getAnalogInput() { + public RevSensor getAnalogInput() { return analogInput; } @Override - public Gyroscope getSelectedSensor() { + public RevSensor getPwmInput() { + return pwmInput; + } + + @Override + public RevSensor getSelectedSensor() { return selectedInput; } + @Override + public void setEncoderCodesPerRevolution(int codesPerRev) { + talon.configEncoderCodesPerRev(codesPerRev); + } + + @Override + public void setPotentiometerTurns(int turns) { + talon.configPotentiometerTurns(turns); + } + @Override public TalonSRX setFeedbackDevice(FeedbackDevice device) { talon.setFeedbackDevice(CANTalon.FeedbackDevice.valueOf(device.value())); - switch(device) { - case ANALOG_POTENTIOMETER: - case ANALOG_ENCODER: - if ( selectedAnalogInput != null ) { - selectedInput = selectedAnalogInput; - } else { - Strongback.logger().error("Unable to use the analog input for feedback, since the Talon SRX (device " + getDeviceID() + ") was not instantiated with an analog input. Check how this device was created using Strongback's Hardware class."); - selectedInput = NO_OP_SENSOR; - } - break; - case QUADRATURE_ENCODER: - case ENCODER_RISING: - if ( selectedEncoderInput != null ) { - selectedInput = selectedEncoderInput; - } else { - Strongback.logger().error("Unable to use the quadrature encoder input for feedback, since the Talon SRX (device " + getDeviceID() + ") was not instantiated with an encoder input. Check how this device was created using Strongback's Hardware class."); - selectedInput = NO_OP_SENSOR; - } - break; - case ENCODER_FALLING: - // for 2015 the Talon SRX firmware did not support the falling or rising mode ... - selectedInput = NO_OP_SENSOR; - break; - case MAGNETIC_ENCODER_ABSOLUTE: - selectedInput = NO_OP_SENSOR; - break; - case MAGNETIC_ENCODER_RELATIVE: - selectedInput = NO_OP_SENSOR; - break; - case PULSE_WIDTH: - selectedInput = NO_OP_SENSOR; - break; - } return this; } @@ -450,12 +265,8 @@ public TemperatureSensor getTemperatureSensor() { } @Override - 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); - } + public TalonSRX setForwardSoftLimit(int forwardLimit) { + talon.setForwardSoftLimit(forwardLimit); return this; } @@ -466,12 +277,8 @@ public HardwareTalonSRX enableForwardSoftLimit(boolean enable) { } @Override - 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); - } + public HardwareTalonSRX setReverseSoftLimit(int reverseLimit) { + talon.setReverseSoftLimit(reverseLimit); return this; }