Merge pull request #66 from rhauch/issue-65

rhauch · rhauch · commit 367ac63d79e9 · 2016-02-20T10:45:21.000-06:00
Issue 65 - Added support for Victor SP hardware motor controllers
diff --git a/strongback-examples/src/org/strongback/example/simple/SimpleTankDriveRobot.java b/strongback-examples/src/org/strongback/example/simple/SimpleTankDriveRobot.java
@@ -22,6 +22,7 @@
 import org.strongback.components.ui.FlightStick;
 import org.strongback.drive.TankDrive;
 import org.strongback.hardware.Hardware;
+import org.strongback.util.Values;
 
 import edu.wpi.first.wpilibj.IterativeRobot;
 
@@ -59,7 +60,7 @@ public void robotInit() {
         // "sensitivity" input to scale the drive speed and throttle, so we'll map it from it's native [-1,1] to a simple scale
         // factor of [0,1] ...
         FlightStick joystick = Hardware.HumanInterfaceDevices.logitechAttack3D(JOYSTICK_PORT);
-        ContinuousRange sensitivity = joystick.getThrottle().map(t -> (t + 1.0) / 2.0);
+        ContinuousRange sensitivity = joystick.getThrottle().map(Values.mapRange(-1.0,1.0).toRange(0.0, 1.0));
         driveSpeed = joystick.getPitch().scale(sensitivity::read); // scaled
         turnSpeed = joystick.getRoll().scale(sensitivity::read).invert(); // scaled and inverted
 
diff --git a/strongback-tests/src/org/strongback/util/ValuesTest.java b/strongback-tests/src/org/strongback/util/ValuesTest.java
@@ -0,0 +1,111 @@
+/*
+ * 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.util;
+
+import static org.fest.assertions.Assertions.assertThat;
+
+import org.fest.assertions.Delta;
+import org.junit.Test;
+import org.strongback.function.DoubleToDoubleFunction;
+
+public class ValuesTest {
+
+    private static final Delta TOLERANCE = Delta.delta(0.00001);
+
+    @Test
+    public void shouldMapRangeFromZeroCenteredToPositiveOne() {
+        DoubleToDoubleFunction func = Values.mapRange(-1.0,1.0,0.0,1.0);
+
+        // Verify minimum range is properly limited ...
+        assertThat(func.applyAsDouble(-1.0)).isEqualTo(0.0, TOLERANCE);
+        assertThat(func.applyAsDouble(-1.01)).isEqualTo(0.0, TOLERANCE);
+        assertThat(func.applyAsDouble(-2.0)).isEqualTo(0.0, TOLERANCE);
+
+        // Verify maximum range is properly limited ...
+        assertThat(func.applyAsDouble(1.0)).isEqualTo(1.0, TOLERANCE);
+        assertThat(func.applyAsDouble(1.01)).isEqualTo(1.0, TOLERANCE);
+        assertThat(func.applyAsDouble(2.0)).isEqualTo(1.0, TOLERANCE);
+
+        // Verify mid-range
+        assertThat(func.applyAsDouble(0.0)).isEqualTo(0.5, TOLERANCE);
+
+        // Verify other values within the range ...
+        assertThat(func.applyAsDouble(-0.75)).isEqualTo(0.125, TOLERANCE);
+        assertThat(func.applyAsDouble(-0.5)).isEqualTo(0.25, TOLERANCE);
+        assertThat(func.applyAsDouble(-0.25)).isEqualTo(0.375, TOLERANCE);
+        assertThat(func.applyAsDouble(0.25)).isEqualTo(0.625, TOLERANCE);
+        assertThat(func.applyAsDouble(0.5)).isEqualTo(0.75, TOLERANCE);
+        assertThat(func.applyAsDouble(0.75)).isEqualTo(0.875, TOLERANCE);
+    }
+
+    @Test
+    public void shouldMapRangeUsingTranslationOnly() {
+        DoubleToDoubleFunction func = Values.mapRange(0.0,4.0,10.0,14.0);
+
+        // Verify minimum range is properly limited ...
+        assertThat(func.applyAsDouble(0.0)).isEqualTo(10.0, TOLERANCE);
+        assertThat(func.applyAsDouble(-0.01)).isEqualTo(10.0, TOLERANCE);
+        assertThat(func.applyAsDouble(-2.0)).isEqualTo(10.0, TOLERANCE);
+
+        // Verify maximum range is properly limited ...
+        assertThat(func.applyAsDouble(4.0)).isEqualTo(14.0, TOLERANCE);
+        assertThat(func.applyAsDouble(4.01)).isEqualTo(14.0, TOLERANCE);
+        assertThat(func.applyAsDouble(6.0)).isEqualTo(14.0, TOLERANCE);
+
+        // Verify mid-range
+        assertThat(func.applyAsDouble(2.0)).isEqualTo(12.0, TOLERANCE);
+
+        // Verify other values within the range ...
+        assertThat(func.applyAsDouble(0.5)).isEqualTo(10.5, TOLERANCE);
+        assertThat(func.applyAsDouble(1.0)).isEqualTo(11.0, TOLERANCE);
+        assertThat(func.applyAsDouble(1.5)).isEqualTo(11.5, TOLERANCE);
+        assertThat(func.applyAsDouble(2.0)).isEqualTo(12.0, TOLERANCE);
+        assertThat(func.applyAsDouble(2.5)).isEqualTo(12.5, TOLERANCE);
+        assertThat(func.applyAsDouble(3.0)).isEqualTo(13.0, TOLERANCE);
+        assertThat(func.applyAsDouble(3.5)).isEqualTo(13.5, TOLERANCE);
+        assertThat(func.applyAsDouble(4.0)).isEqualTo(14.0, TOLERANCE);
+    }
+
+    @Test
+    public void shouldMapRangeUsingScaleOnly() {
+        DoubleToDoubleFunction func = Values.mapRange(1.0,5.0).toRange(1.0,2.0);
+
+        // Verify minimum range is properly limited ...
+        assertThat(func.applyAsDouble(0.0)).isEqualTo(1.0, TOLERANCE);
+        assertThat(func.applyAsDouble(-0.01)).isEqualTo(1.0, TOLERANCE);
+        assertThat(func.applyAsDouble(-2.0)).isEqualTo(1.0, TOLERANCE);
+
+        // Verify maximum range is properly limited ...
+        assertThat(func.applyAsDouble(5.0)).isEqualTo(2.0, TOLERANCE);
+        assertThat(func.applyAsDouble(5.01)).isEqualTo(2.0, TOLERANCE);
+        assertThat(func.applyAsDouble(5.0)).isEqualTo(2.0, TOLERANCE);
+
+        // Verify mid-range
+        assertThat(func.applyAsDouble(3.0)).isEqualTo(1.5, TOLERANCE);
+
+        // Verify other values within the range ...
+        assertThat(func.applyAsDouble(1.04)).isEqualTo(1.01, TOLERANCE);
+        assertThat(func.applyAsDouble(1.8)).isEqualTo(1.2, TOLERANCE);
+        assertThat(func.applyAsDouble(2.0)).isEqualTo(1.25, TOLERANCE);
+        assertThat(func.applyAsDouble(2.6)).isEqualTo(1.4, TOLERANCE);
+        assertThat(func.applyAsDouble(3.4)).isEqualTo(1.6, TOLERANCE);
+        assertThat(func.applyAsDouble(3.8)).isEqualTo(1.7, TOLERANCE);
+        assertThat(func.applyAsDouble(4.2)).isEqualTo(1.8, TOLERANCE);
+        assertThat(func.applyAsDouble(4.6)).isEqualTo(1.9, TOLERANCE);
+    }
+
+}
diff --git a/strongback/src/org/strongback/hardware/Hardware.java b/strongback/src/org/strongback/hardware/Hardware.java
@@ -60,6 +60,7 @@
 import edu.wpi.first.wpilibj.Talon;
 import edu.wpi.first.wpilibj.Ultrasonic;
 import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.VictorSP;
 import edu.wpi.first.wpilibj.interfaces.Accelerometer.Range;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
 
@@ -545,6 +546,29 @@ public static Motor victor(int channel, DoubleToDoubleFunction speedLimiter) {
             return new HardwareMotor(new Victor(channel), SPEED_LIMITER);
         }
 
+        /**
+         * Create a motor driven by a VEX Robotics Victor SP speed controller on the specified channel. The speed output is
+         * limited to [-1.0,1.0] inclusive.
+         *
+         * @param channel the channel the controller is connected to
+         * @return a motor on the specified channel
+         */
+        public static Motor victorSP(int channel) {
+            return victorSP(channel, SPEED_LIMITER);
+        }
+
+        /**
+         * Create a motor driven by a VEX Robotics Victor SP speed controller on the specified channel, with a custom speed
+         * limiting function.
+         *
+         * @param channel the channel the controller is connected to
+         * @param speedLimiter function that will be used to limit the speed (input voltage); may not be null
+         * @return a motor on the specified channel
+         */
+        public static Motor victorSP(int channel, DoubleToDoubleFunction speedLimiter) {
+            return new HardwareMotor(new VictorSP(channel), SPEED_LIMITER);
+        }
+
         /**
          * Creates a {@link TalonSRX} motor controlled by a Talon SRX with no sensors wired as inputs.
          * <p>
diff --git a/strongback/src/org/strongback/util/Values.java b/strongback/src/org/strongback/util/Values.java
@@ -17,7 +17,6 @@
 
 import org.strongback.function.DoubleToDoubleFunction;
 
-
 /**
  * Utility class for working with values.
  *
@@ -75,10 +74,12 @@ private static double calcTolerance(int bits) {
      * @throws IllegalArgumentException if the tolerance is negative
      */
     public static int fuzzyCompare(double a, double b, double tolerance) {
-        if ( tolerance < 0.0 ) throw new IllegalArgumentException("The tolerance may not be negative");
+        if (tolerance < 0.0) throw new IllegalArgumentException("The tolerance may not be negative");
         double difference = a - b;
-        return (Math.abs(difference) <= tolerance ? 0 : // the two values are within the tolerance of each other
-                (difference > 0 ? 1 : // the first is greater than the second
+        return (Math.abs(difference) <= tolerance ? 0
+                : // the two values are within the tolerance of each other
+                (difference > 0 ? 1
+                        : // the first is greater than the second
                         -1)); // the first is less than the second
     }
 
@@ -92,7 +93,8 @@ public static int fuzzyCompare(double a, double b, double tolerance) {
      * @throws IllegalArgumentException if the minimum value is greater than the maximum value
      */
     public static double limit(double minimum, double num, double maximum) {
-        if ( maximum < minimum ) throw new IllegalArgumentException("The minimum value must be less than or equal to the maximum value");
+        if (maximum < minimum) throw new IllegalArgumentException(
+                "The minimum value must be less than or equal to the maximum value");
         if (num > maximum) {
             return maximum;
         }
@@ -112,7 +114,8 @@ public static double limit(double minimum, double num, double maximum) {
      * @throws IllegalArgumentException if the minimum value is greater than the maximum value
      */
     public static DoubleToDoubleFunction limiter(double minimum, double maximum) {
-        if ( maximum < minimum ) throw new IllegalArgumentException("The minimum value must be less than or equal to the maximum value");
+        if (maximum < minimum) throw new IllegalArgumentException(
+                "The minimum value must be less than or equal to the maximum value");
         return new DoubleToDoubleFunction() {
             @Override
             public double applyAsDouble(double value) {
@@ -138,9 +141,10 @@ public double applyAsDouble(double value) {
      * @throws IllegalArgumentException if the minimum and maximum values are invalid
      */
     public static double symmetricLimit(double minimum, double num, double maximum) {
-        if ( minimum < 0 ) throw new IllegalArgumentException("The minimum value may not be negative");
-        if ( maximum < 0 ) throw new IllegalArgumentException("The maximum value may not be negative");
-        if ( maximum < minimum ) throw new IllegalArgumentException("The minimum value must be less than or equal to the maximum value");
+        if (minimum < 0) throw new IllegalArgumentException("The minimum value may not be negative");
+        if (maximum < 0) throw new IllegalArgumentException("The maximum value may not be negative");
+        if (maximum < minimum) throw new IllegalArgumentException(
+                "The minimum value must be less than or equal to the maximum value");
         if (num > maximum) {
             return maximum;
         }
@@ -160,9 +164,10 @@ public static double symmetricLimit(double minimum, double num, double maximum)
      * @return the function that limits to the maximum and minimum values; never null
      */
     public static DoubleToDoubleFunction symmetricLimiter(double minimum, double maximum) {
-        if ( minimum < 0 ) throw new IllegalArgumentException("The minimum value may not be negative");
-        if ( maximum < 0 ) throw new IllegalArgumentException("The maximum value may not be negative");
-        if ( maximum < minimum ) throw new IllegalArgumentException("The minimum value must be less than or equal to the maximum value");
+        if (minimum < 0) throw new IllegalArgumentException("The minimum value may not be negative");
+        if (maximum < 0) throw new IllegalArgumentException("The maximum value may not be negative");
+        if (maximum < minimum) throw new IllegalArgumentException(
+                "The minimum value must be less than or equal to the maximum value");
         return new DoubleToDoubleFunction() {
             @Override
             public double applyAsDouble(double num) {
@@ -178,4 +183,63 @@ public double applyAsDouble(double num) {
         };
     }
 
+    public static interface RangeMaker {
+        DoubleToDoubleFunction toRange(double minOutputValue, double maxOutputValue);
+    }
+
+    /**
+     * Begin to create a function that maps from the specified range of input values. To obtain a mapping function, call the
+     * {@link RangeMaker#toRange(double, double)} method on the resulting {@link RangeMaker} instance.
+     * <p>
+     * This is equivalent to calling {@link #mapRange(double, double, double, double)} with the input and output range limits.
+     * For example:
+     *
+     * <pre>
+     * Values.mapRange(-1.0,1.0).toRange(0.0,1.0);
+     * </pre>
+     *
+     * is equivalent to:
+     *
+     * <pre>
+     * Values.mapRange(-1.0,1.0,0.0,1.0);
+     * </pre>
+     *
+     * @param minInputValue the minimum value of the range of input values to the function
+     * @param maxInputValue the maximum value of the range of input values to the function
+     * @return the range maker that should be used to complete the function; never null
+     * @see #mapRange(double, double, double, double)
+     */
+    public static RangeMaker mapRange(double minInputValue, double maxInputValue) {
+        return (minOutput, maxOutput) -> {
+            return mapRange(minInputValue, maxInputValue, minOutput, maxOutput);
+        };
+    }
+
+    /**
+     * Create a function that maps from the specified range of input values
+     *
+     * @param minInputValue the minimum value of the range of input values to the function
+     * @param maxInputValue the maximum value of the range of input values to the function
+     * @param minOutputValue the minimum value for the output of the function
+     * @param maxOutputValue the maximum value for the output of the function
+     * @return the mapping function; never null
+     * @see #mapRange(double, double)
+     */
+    public static DoubleToDoubleFunction mapRange(double minInputValue, double maxInputValue, double minOutputValue,
+            double maxOutputValue) {
+        double factor = (maxOutputValue - minOutputValue) / (maxInputValue - minInputValue);
+        return new DoubleToDoubleFunction() {
+            @Override
+            public double applyAsDouble(double num) {
+                if (num <= minInputValue) return minOutputValue;
+                if (num >= maxInputValue) return maxOutputValue;
+                double output = minOutputValue + ((num - minInputValue) * factor);
+                if (output < minOutputValue)
+                    output = minOutputValue;
+                else if (output > maxOutputValue) output = maxOutputValue;
+                return output;
+            }
+        };
+    }
+
 }
