Added a TalonController to work with hardware-based PID control on Talon SRX devices

build.properties

@@ -1,4 +1,4 @@
-strongback.version=1.1.0-Alpha2-SNAPSHOT
+strongback.version=1.1.0-Alpha3-SNAPSHOT
 #
 # The build will download a specific version of the WPILib given by the following URL
 # and install it into the 'libs/wpilib' folder. To use a different version of WPILib,

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

@@ -16,7 +16,12 @@
 
 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;
+import org.strongback.control.PIDController;
 
 /**
  * Factory for mock components.
@@ -25,6 +30,8 @@
  */
 public class Mock {
 
+    private static final AtomicLong CAN_DEVICE_ID_GENERATOR = new AtomicLong();
+
     /**
      * Create a mock power panel.
      *
@@ -230,21 +237,64 @@ public static MockMotor runningMotor(double speed) {
     }
 
     /**
-     * Create a stopped mock Talon SRX motor.
+     * Create a stopped mock {@link TalonSRX} motor.
+     *
+     * @param deviceId the CAN device ID
+     * @return the mock TalonSRX motor; never null
+     */
+    public static MockTalonSRX stoppedTalonSRX(int deviceId) {
+        return runningTalonSRX(deviceId, 0.0);
+    }
+
+    /**
+     * Create a running mock {@link TalonSRX} motor.
+     *
+     * @param deviceId the CAN device ID
+     * @param speed the initial speed
+     * @return the mock TalonSRX motor; never null
+     */
+    public static MockTalonSRX runningTalonSRX(int deviceId, double speed) {
+        return new MockTalonSRX(deviceId, speed);
+    }
+
+    /**
+     * Create a stopped mock {@link TalonSRX} motor.
      *
      * @return the mock TalonSRX motor; never null
      */
     public static MockTalonSRX stoppedTalonSRX() {
-        return new MockTalonSRX(0.0);
+        return stoppedTalonSRX(nextDeviceId());
     }
 
     /**
-     * Create a running mock Talon SRX motor.
+     * Create a running mock {@link TalonSRX} motor.
      *
      * @param speed the initial speed
      * @return the mock TalonSRX motor; never null
      */
     public static MockTalonSRX runningTalonSRX(double speed) {
-        return new MockTalonSRX(speed);
+        return runningTalonSRX(nextDeviceId(), speed);
+    }
+
+    protected static int nextDeviceId() {
+        return (int)CAN_DEVICE_ID_GENERATOR.getAndIncrement();
+    }
+
+    /**
+     * Create a mock {@link Controller}.
+     *
+     * @return the mock controller; never null
+     */
+    public static MockController controller() {
+        return new MockController();
+    }
+
+    /**
+     * Create a mock {@link PIDController}.
+     *
+     * @return the mock controller; never null
+     */
+    public static MockPIDController pidController() {
+        return new MockPIDController();
     }
 }

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

@@ -0,0 +1,102 @@
+/*
+ * 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.mock;
+
+import org.strongback.Executable;
+import org.strongback.control.Controller;
+
+/**
+ * A {@link Controller} implementation that tests can use to manually {@link #setValue(double) set the value}.
+ */
+public class MockController implements Controller {
+
+    private boolean enabled = true;
+    private double setpoint = 0.0d;
+    private double tolerance = 0.0d;
+    private double value = 0.0d;
+    private final Executable executable = (time)->computeOutput();
+
+    @Override
+    public boolean isEnabled() {
+        return enabled;
+    }
+
+    @Override
+    public Controller enable() {
+        enabled = true;
+        return this;
+    }
+
+    @Override
+    public Controller disable() {
+        enabled = false;
+        return this;
+    }
+
+    @Override
+    public double getTarget() {
+        return setpoint;
+    }
+
+    @Override
+    public MockController withTarget(double target) {
+        this.setpoint = target;
+        return this;
+    }
+
+    @Override
+    public double getTolerance() {
+        return tolerance;
+    }
+
+    @Override
+    public MockController withTolerance(double tolerance) {
+        this.tolerance = tolerance;
+        return this;
+    }
+
+    @Override
+    public boolean computeOutput() {
+        return isWithinTolerance();
+    }
+
+    @Override
+    public MockController reset() {
+        return this;
+    }
+
+    @Override
+    public boolean hasExecutable() {
+        return true;
+    }
+
+    @Override
+    public Executable executable() {
+        return executable;
+    }
+
+    @Override
+    public double getValue() {
+        return value;
+    }
+
+    public MockController setValue( double value ) {
+        this.value = value;
+        return this;
+    }
+
+}

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

@@ -0,0 +1,112 @@
+/*
+ * 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.mock;
+
+import java.util.Collections;
+import java.util.Map;
+import java.util.Set;
+import java.util.concurrent.ConcurrentHashMap;
+
+import org.strongback.control.Controller;
+import org.strongback.control.PIDController;
+
+/**
+ * A {@link Controller} implementation that tests can use to manually {@link #setValue(double) set the value}.
+ */
+public class MockPIDController extends MockController implements PIDController {
+
+    private volatile int currentProfile = PIDController.DEFAULT_PROFILE;
+    private final Map<Integer,Gains> profiles = new ConcurrentHashMap<>();
+
+    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());
+    }
+
+    @Override
+    public synchronized MockPIDController withProfile(int profile, double p, double i, double d, double feedForward) {
+        profiles.put(profile, new Gains(p,i,d,feedForward));
+        return this;
+    }
+
+    @Override
+    public synchronized int getCurrentProfile() {
+        return currentProfile;
+    }
+
+    @Override
+    public synchronized Gains getGainsForCurrentProfile() {
+        return profiles.get(currentProfile);
+    }
+
+    @Override
+    public synchronized MockPIDController useProfile(int profile) {
+        if ( !profiles.containsKey(profile) ) throw new IllegalArgumentException("Invalid profile");
+        currentProfile = profile;
+        return this;
+    }
+
+    protected final class Gains implements PIDController.Gains {
+        protected final double p;
+        protected final double i;
+        protected final double d;
+        protected final double feedForward;
+
+        protected Gains(double p, double i, double d, double feedForward) {
+            this.p = p;
+            this.i = i;
+            this.d = d;
+            this.feedForward = feedForward;
+        };
+
+        @Override
+        public double getP() {
+            return p;
+        }
+
+        @Override
+        public double getI() {
+            return i;
+        }
+
+        @Override
+        public double getD() {
+            return d;
+        }
+
+        @Override
+        public double getFeedForward() {
+            return feedForward;
+        }
+    }
+}