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

Author: rhauch

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

@@ -247,4 +247,12 @@ public static MockTalonSRX stoppedTalonSRX() {
     public static MockTalonSRX runningTalonSRX(double speed) {
         return new MockTalonSRX(speed);
     }
+
+    /**
+     * Create a mock controller.
+     * @return the mock controller; never null
+     */
+    public static MockController controller() {
+        return new MockController();
+    }
 }

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

@@ -0,0 +1,105 @@
+/*
+ * 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 #setError(double) set the error}.
+ *
+ * @author Randall Hauch
+ */
+public class MockController implements Controller {
+
+    private boolean enabled = true;
+    private double setpoint = 0.0d;
+    private double tolerance = 0.0d;
+    private double error = 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 getSetpoint() {
+        return setpoint;
+    }
+
+    @Override
+    public Controller setpoint(double setpoint) {
+        this.setpoint = setpoint;
+        return this;
+    }
+
+    @Override
+    public Controller tolerance(double tolerance) {
+        this.tolerance = tolerance;
+        return this;
+    }
+
+    @Override
+    public boolean withinTolerance() {
+        // Determine if we're within the tolerance ...
+        return Math.abs(error) < tolerance;
+    }
+
+    @Override
+    public boolean checkTolerance(double value) {
+        return Math.abs(value) <= (setpoint - tolerance);
+    }
+
+    @Override
+    public boolean computeOutput() {
+        return withinTolerance();
+    }
+
+    @Override
+    public Controller reset() {
+        error = 0.0;
+        return this;
+    }
+
+    @Override
+    public boolean hasExecutable() {
+        return true;
+    }
+
+    @Override
+    public Executable executable() {
+        return executable;
+    }
+
+    public MockController setError( double error ) {
+        this.error = error;
+        return this;
+    }
+}

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

@@ -17,6 +17,8 @@
 package org.strongback.mock;
 
 import org.strongback.components.TalonSRX;
+import org.strongback.components.TemperatureSensor;
+import org.strongback.components.VoltageSensor;
 
 /**
  * @author Randall Hauch
@@ -26,6 +28,9 @@ public class MockTalonSRX extends MockMotor implements TalonSRX {
 
     private final MockAngleSensor encoder = new MockAngleSensor();
     private final MockCurrentSensor current = new MockCurrentSensor();
+    private final MockVoltageSensor voltage = new MockVoltageSensor();
+    private final MockVoltageSensor busVoltage = new MockVoltageSensor();
+    private final MockTemperatureSensor temperature = new MockTemperatureSensor();
     private final MockSwitch forwardLimitSwitch = new MockSwitch();
     private final MockSwitch reverseLimitSwitch = new MockSwitch();
 
@@ -49,6 +54,21 @@ public MockCurrentSensor getCurrentSensor() {
         return current;
     }
 
+    @Override
+    public VoltageSensor getVoltageSensor() {
+        return voltage;
+    }
+
+    @Override
+    public VoltageSensor getBusVoltageSensor() {
+        return busVoltage;
+    }
+
+    @Override
+    public TemperatureSensor getTemperatureSensor() {
+        return temperature;
+    }
+
     @Override
     public MockSwitch getForwardLimitSwitch() {
         return forwardLimitSwitch;

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

@@ -34,10 +34,31 @@ public interface TalonSRX extends LimitedMotor {
     public AngleSensor getAngleSensor();
 
     /**
-     * Get the Talon SRX's current sensor.
+     * Get the Talon SRX's output current sensor.
      *
-     * @return the current sensor; never null
+     * @return the output current sensor; never null
      */
     public CurrentSensor getCurrentSensor();
 
+    /**
+     * Get the Talon SRX's output voltage sensor.
+     *
+     * @return the output voltage sensor; never null
+     */
+    public VoltageSensor getVoltageSensor();
+
+    /**
+     * Get the Talon SRX's bus voltage.
+     *
+     * @return the bus voltage sensor; never null
+     */
+    public VoltageSensor getBusVoltageSensor();
+
+    /**
+     * Get the Talon SRX's temperature sensor.
+     *
+     * @return the temperature sensor; never null
+     */
+    public TemperatureSensor getTemperatureSensor();
+
 }

strongback/src/org/strongback/control/Controller.java

@@ -16,6 +16,10 @@
 
 package org.strongback.control;
 
+import org.strongback.Executable;
+import org.strongback.Executor;
+import org.strongback.Strongback;
+
 /**
  * A basic controller.
  *
@@ -24,28 +28,41 @@
 public interface Controller {
 
     /**
-     * Calculate the next controller output. This usually involves reading one or more inputs and computing the output based
-     * upon a model of the system.
+     * Determine if this controller is {@link #enable() enabled}.
      *
-     * @return <code>true</code> if the execution completed because it resulted in a value that is within the tolerance of the
-     *         setpoint, or <code>false</code> if this controller is not {@link #isEnabled() enabled} or has not yet reached the
-     *         setpoint given the tolerance
+     * @return <code>true</code> if enabled, or <code>false</code> otherwise
      */
-    public boolean computeOutput();
+    public boolean isEnabled();
 
     /**
-     * Determines whether the supplied value is within the tolerance of the setpoint.
+     * Enable this controller so that it does read inputs, compute errors, and generate outputs when {@link #computeOutput()} is
+     * called.
      *
-     * @param value the proposed value
-     * @return <code>true</code> if the proposed value is within the tolerance of the setpoint, or <code>false</code> otherwise
+     * @return this object so that methods can be chained; never null
      */
-    public boolean checkTolerance(double value);
+    public Controller enable();
+
+    /**
+     * Disable this controller to <em>not</em> read inputs, compute errors, and generate outputs when {@link #computeOutput()}
+     * is called.
+     *
+     * @return this object so that methods can be chained; never null
+     */
+    public Controller disable();
+
+    /**
+     * Get the target value for this controller.
+     * @return the target value
+     * @see #setpoint(double)
+     */
+    public double getSetpoint();
 
     /**
      * Sets the target value for this controller.
      *
      * @param setpoint the desired setpoint that this controller will use as a target
      * @return this object so that methods can be chained; never null
+     * @see #getSetpoint()
      */
     public Controller setpoint(double setpoint);
 
@@ -58,33 +75,57 @@ public interface Controller {
     public Controller tolerance(double tolerance);
 
     /**
-     * Reset any error values stored from previous {@link #computeOutput() executions}.
+     * Determines whether the current value is within the tolerance of the setpoint.
      *
-     * @return this object so that methods can be chained; never null
+     * @return <code>true</code> if the current value is within the tolerance of the setpoint, or <code>false</code> otherwise
+     * @see #checkTolerance(double)
      */
-    public Controller reset();
+    public boolean withinTolerance();
 
     /**
-     * Determine if this controller is {@link #enable() enabled}.
+     * Determines whether the supplied value is within the tolerance of the setpoint.
      *
-     * @return <code>true</code> if enabled, or <code>false</code> otherwise
+     * @param value the proposed value
+     * @return <code>true</code> if the proposed value is within the tolerance of the setpoint, or <code>false</code> otherwise
+     * @see #withinTolerance()
      */
-    public boolean isEnabled();
+    public boolean checkTolerance(double value);
 
     /**
-     * Enable this controller so that it does read inputs, compute errors, and generate outputs when {@link #computeOutput()} is
-     * called.
+     * Calculate the next controller output. This usually involves reading one or more inputs and computing the output based
+     * upon a model of the system.
      *
-     * @return this object so that methods can be chained; never null
+     * @return <code>true</code> if the execution completed because it resulted in a value that is within the tolerance of the
+     *         setpoint, or <code>false</code> if this controller is not {@link #isEnabled() enabled} or has not yet reached the
+     *         setpoint given the tolerance
      */
-    public Controller enable();
+    public boolean computeOutput();
 
     /**
-     * Disable this controller to <em>not</em> read inputs, compute errors, and generate outputs when {@link #computeOutput()}
-     * is called.
+     * Reset any error values stored from previous {@link #computeOutput() executions}.
      *
      * @return this object so that methods can be chained; never null
      */
-    public Controller disable();
+    public Controller reset();
+
+    /**
+     * Return whether the {@link Executable} instance returned by {@link #executable()} does anything useful. Software-based
+     * controllers may often return <code>true</code>, whereas many hardware-based controllers may return <code>false</code>.
+     * @return <code>true</code> if the {@link #executable()} does useful work and enables the controller to operate
+     * continually and automatically, or <code>false</code> if the {@link #executable()} does nothing.
+     */
+    public boolean hasExecutable();
+
+    /**
+     * Get the {@link Executable} instance that will continuously {@link #computeOutput() execute} this controller to read
+     * inputs from the source and generate outputs to reach the {@link #setpoint(double) setpoint}.
+     * <p>
+     * If this is used, then this same controller should <em>never</em> be used with commands. This is not checked, so robot
+     * programs are responsible for ensuring this does not happen.
+     *
+     * @return the {@link Executable} object that can be registered with an {@link Executor} (typically Strongback's
+     *         {@link Strongback#executor() central executor}); never null and always the same instance for this controller
+     */
+    public Executable executable();
 
 }