Added unit tests for the computed Gyroscopes in the HardwareTalonSRX …

…class. The unit tests verify the basic logic, but still don't match a real hardware test.

strongback-testing/src/org/strongback/mock/MockPIDController.java

@@ -36,6 +36,18 @@ public MockPIDController() {
         withGains(0.0,0.0,0.0);
     }
 
+    @Override
+    public MockPIDController withTarget(double target) {
+        super.withTarget(target);
+        return this;
+    }
+
+    @Override
+    public MockPIDController withTolerance(double tolerance) {
+        super.withTolerance(tolerance);
+        return this;
+    }
+
     @Override
     public Set<Integer> getProfiles() {
         return Collections.unmodifiableSet(profiles.keySet());

strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_AnalogInputSensorTest.java

@@ -0,0 +1,119 @@
+/*
+ * 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));
+    }
+
+}

strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_EncoderInputSensorTest.java

@@ -0,0 +1,126 @@
+/*
+ * 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);
+    }
+
+}

strongback/src/org/strongback/components/TalonSRX.java

@@ -297,18 +297,27 @@ public enum FeedbackDevice {
         /**
          * Use Quadrature Encoder.
          */
-        QUADRATURE_ENCODER(0), /**
-                                * Analog potentiometer, 0-3.3V
-                                */
-        ANALOG_POTENTIOMETER(2), /**
-                                  * Analog encoder or any other analog device, 0-3.3V
-                                  */
-        ANALOG_ENCODER(3), /**
-                            * Encoder that increments position per rising edge (and never decrements) on Quadrature-A.
-                            */
-        ENCODER_RISING(4), /**
-                            * Encoder that increments position per falling edge (and never decrements) on Quadrature-A.
-                            */
+        QUADRATURE_ENCODER(0),
+
+        /**
+         * Analog potentiometer or any other analog device, 0-3.3V
+         */
+        ANALOG_POTENTIOMETER(2),
+
+        /**
+         * Analog encoder or any other analog device, 0-3.3V
+         */
+        ANALOG_ENCODER(3),
+
+        /**
+         * Encoder that increments position per rising edge (and never decrements) on Quadrature-A.
+         */
+        ENCODER_RISING(4),
+
+        /**
+         * Encoder that increments position per falling edge (and never decrements) on Quadrature-A.
+         * Note: Was not supported in 2015 firmware, per the <a href="https://www.ctr-electronics.com/Talon%20SRX%20Software%20Reference%20Manual.pdf">Talon SRX Software Reference Manual</a>, section 21.3
+         */
         ENCODER_FALLING(5);
 
         public int value;
@@ -335,7 +344,65 @@ public int value() {
      * Types of status frame rates.
      */
     public enum StatusFrameRate {
-        GENERAL(0), FEEDBACK(1), QUADRATURE_ENCODER(2), ANALOG_TEMPERATURE_BATTERY_VOLTAGE(3);
+        /**
+         * The General Status frame has a default period of 10ms, and provides:
+         * <ul>
+         * <li>Closed Loop Error - the closed-loop target minus actual position/velocity.</li>
+         * <li>Throttle - the current 10bit motor output duty cycle (-1023 full reverse to +1023 full forward).</li>
+         * <li>Forward Limit Switch Pin State</li>
+         * <li>Reverse Limit Switch Pin State</li>
+         * <li>Fault bits</li>
+         * <li>Applied Control Mode</li>
+         * </ul>
+         */
+        GENERAL(0),
+
+        /**
+         * The Feedback Status frame has a default period of 20ms, and provides:
+         * <ul>
+         * <li>Sensor Position - position of the selected sensor</li>
+         * <li>Sensor Velocity - velocity of the selected sensor</li>
+         * <li>Motor Current</li>
+         * <li>Sticky Faults</li>
+         * <li>Brake Neutral State</li>
+         * <li>Motor Control Profile Select</li>
+         * </ul>
+         */
+        FEEDBACK(1),
+
+        /**
+         * The Quadrature Encoder Status frame has a default period of 100ms, and provides:
+         * <ul>
+         * <li>Encoder Position - position of the quadrature sensor</li>
+         * <li>Encoder Velocity - velocity of the selected sensor</li>
+         * <li>Number of rising edges counted on the Index Pin.</li>
+         * <li>Quad A pin state.</li>
+         * <li>Quad B pin state.</li>
+         * <li>Quad Index pin state.</li>
+         * </ul>
+         * The quadrature decoder is always engaged, whether the feedback device is selected or not, and whether a quadrature
+         * encoder is actually wired or not. This means that the Quadrature Encoder signals are always available in programming
+         * API regardless of how the Talon is used. The 100ms update rate is sufficient for logging, instrumentation and
+         * debugging. If a faster update rate is required the robot application can select the appropriate sensor and leverage
+         * the Sensor Position and Sensor Velocity sent over the {@link #FEEDBACK} frame every 20ms (by default).
+         */
+        QUADRATURE_ENCODER(2),
+
+        /**
+         * The Analog, Temperature, and Battery Voltage status frame has a default period of 100ms, and provides:
+         * <ul>
+         * <li>Analog Position - position of the analog sensor</li>
+         * <li>Analog Velocity - velocity of the analog sensor</li>
+         * <li>Temperature</li>
+         * <li>Battery Voltage</li>
+         * </ul>
+         * The Analog to Digital Converter is always engaged, whether the feedback device is selected or not, and whether an
+         * analog sensor is actually wired or not. This means that the Analog In signals are always available in programming API
+         * regardless of how the Talon is used. The 100ms update rate is sufficient for logging, instrumentation and debugging.
+         * If a faster update rate is required the robot application can select the appropriate sensor and leverage the Sensor
+         * Position and Sensor Velocity sent over the {@link #FEEDBACK} frame every 20ms (by default).
+         */
+        ANALOG_TEMPERATURE_BATTERY_VOLTAGE(3);
         public int value;
 
         public static StatusFrameRate valueOf(int value) {

strongback/src/org/strongback/control/PIDController.java

@@ -41,6 +41,12 @@ public interface PIDController extends Controller {
      */
     public static int DEFAULT_PROFILE = 0;
 
+    @Override
+    public PIDController withTarget(double target);
+
+    @Override
+    public PIDController withTolerance(double tolerance);
+
     /**
      * Set the control gains on this controller's current profile.
      *