Add PID control functionality

strongback-tests/src/org/strongback/control/PIDControllerTest.java

@@ -0,0 +1,165 @@
+/*
+ * 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.control;
+
+import static org.fest.assertions.Assertions.assertThat;
+
+import org.junit.Before;
+import org.junit.Test;
+import org.strongback.function.DoubleBiFunction;
+
+import edu.wpi.first.wpilibj.HLUsageReporting;
+
+/**
+ * @author Randall Hauch
+ *
+ */
+public class PIDControllerTest {
+
+    private static class SystemModel {
+        protected final DoubleBiFunction model;
+        protected boolean print = false;
+
+        public SystemModel(DoubleBiFunction model) {
+            this.model = model;
+        }
+
+        protected double actualValue = 0;
+
+        public double getActualValue() {
+            return actualValue;
+        }
+
+        public void setValue(double input) {
+            double newValue = model.applyAsDouble(actualValue, input);
+            if ( print ) System.out.println("actual=" + actualValue + "; input=" + input + "; newValue=" + newValue);
+            this.actualValue = newValue;
+        }
+    }
+
+    public static SystemModel simple() {
+        return new SystemModel((actual, newValue) -> actual + newValue);
+    }
+
+    public static SystemModel linear(double factor) {
+        return new SystemModel((actual, newValue) -> {
+            return factor * (newValue - actual) + actual;
+        });
+    }
+
+    private static boolean print = false;
+    private SystemModel model;
+    private PIDController controller;
+    private edu.wpi.first.wpilibj.PIDController wpi;
+
+    @Before
+    public void beforeEach() {
+        print = false;
+    }
+
+    @Test
+    public void shouldUseProportionalOnly() {
+        model = simple();
+        //model.print = true;
+        controller = new PIDController(model::getActualValue, model::setValue).withGains(0.9, 0.0, 0.0)
+                                                                              .inputRange(-1.0, 1.0)
+                                                                              .outputRange(-1.0, 1.0)
+                                                                              .tolerance(0.02)
+                                                                              .setpoint(0.5);
+        assertThat(runController(10)).isLessThan(5);
+        assertThat(model.getActualValue()-0.5 < 0.02).isTrue();
+    }
+
+    @Test
+    public void shouldUseProportionalAndDifferential() {
+        model = simple();
+        // model.print = true;
+        controller = new PIDController(model::getActualValue, model::setValue).withGains(0.7, 0.0, 0.3)
+                                                                              .inputRange(-1.0, 1.0)
+                                                                              .outputRange(-1.0, 1.0)
+                                                                              .tolerance(0.02)
+                                                                              .setpoint(0.5);
+        assertThat(runController(10)).isLessThan(5);
+        assertThat(model.getActualValue()-0.5 < 0.02).isTrue();
+    }
+
+
+    @Test
+    public void shouldUseProportionalOnlyWithInitialValue() {
+        model = simple();
+        model.setValue(0.2);
+        //model.print = true;
+        controller = new PIDController(model::getActualValue, model::setValue).withGains(0.9, 0.0, 0.0)
+                                                                              .inputRange(-1.0, 1.0)
+                                                                              .outputRange(-1.0, 1.0)
+                                                                              .tolerance(0.02)
+                                                                              .setpoint(0.5);
+        assertThat(runController(10)).isLessThan(5);
+        assertThat(model.getActualValue()-0.5 < 0.02).isTrue();
+    }
+
+    @Test
+    public void shouldUseProportionalAndDifferentialWithInitialValue() {
+        model = simple();
+        model.setValue(0.2);
+        //model.print = true;
+        controller = new PIDController(model::getActualValue, model::setValue).withGains(0.7, 0.0, 0.3)
+                                                                              .inputRange(-1.0, 1.0)
+                                                                              .outputRange(-1.0, 1.0)
+                                                                              .tolerance(0.02)
+                                                                              .setpoint(0.5);
+        assertThat(runController(10)).isLessThan(5);
+        assertThat(model.getActualValue()-0.5 < 0.02).isTrue();
+    }
+
+    @Test
+    public void shouldUseProportionalOnlyWPILib() throws InterruptedException {
+        HLUsageReporting.SetImplementation(new HLUsageReporting.Interface(){
+            @Override
+            public void reportPIDController(int num) {
+            }
+            @Override
+            public void reportScheduler() {
+            }
+            @Override
+            public void reportSmartDashboard() {
+            }
+        });
+        model = simple();
+        model.setValue(0.30);
+        // model.print = true;
+        wpi = new edu.wpi.first.wpilibj.PIDController(0.9, 0.0, 0.0, model::getActualValue, model::setValue);
+        wpi.setSetpoint(0.5);
+        wpi.setAbsoluteTolerance(0.02);
+        wpi.setInputRange(-1.0, 1.0);
+        wpi.setOutputRange(-1.0, 1.0);
+        wpi.enable();
+        Thread.sleep(300);
+        wpi.disable();
+        assertThat(model.getActualValue()-0.5 < 0.02).isTrue();
+    }
+
+    protected int runController( int maxNumSteps) {
+        int counter = 0;
+        while ( !controller.computeOutput() && counter < maxNumSteps) {
+            ++counter;
+            if ( print ) System.out.println(counter + " " + model.getActualValue());
+        }
+        return counter;
+    }
+
+}

strongback/src/org/strongback/DataRecordable.java

@@ -0,0 +1,33 @@
+/*
+ * 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;
+
+/**
+ * An interface that classes can implement when they are to be recordable but they involve multiple channels.
+ */
+@FunctionalInterface
+public interface DataRecordable {
+
+    /**
+     * Register this object with the specified data recorder.
+     *
+     * @param recorder the data recorder; never null
+     * @param name the parent name for the channels; never null
+     */
+    void registerWith(DataRecorder recorder, String name);
+
+}

strongback/src/org/strongback/DataRecorder.java

@@ -73,4 +73,15 @@ default public DataRecorder register(String name, double scale, DoubleSupplier s
      * @throws IllegalArgumentException if the {@code sensor} parameter is null
      */
     public DataRecorder register(String name, SpeedSensor sensor);
+
+    /**
+     * Registers by name a recordable object that likely has multiple channels. This method will remove any
+     * previously-registered supplier, switch, or motor with the same name.
+     *
+     * @param name the name of the {@link SpeedSensor}
+     * @param recordable the {@link DataRecordable} to be registered
+     * @return this instance so methods can be chained together; never null
+     * @throws IllegalArgumentException if the {@code sensor} parameter is null
+     */
+    public DataRecorder register(String name, DataRecordable recordable);
 }

strongback/src/org/strongback/DataRecorderChannels.java

@@ -63,6 +63,14 @@ public DataRecorder register(String name, SpeedSensor sensor) {
         return this;
     }
 
+    @Override
+    public DataRecorder register(String name, DataRecordable recordable) {
+        if (name == null) throw new IllegalArgumentException("The name may not be null");
+        if (recordable == null) throw new IllegalArgumentException("The recordable may not be null");
+        recordable.registerWith(this, name);
+        return this;
+    }
+
     @Override
     public Iterator<DataRecorderChannel> iterator() {
         return Iterators.immutable(channels);

strongback/src/org/strongback/command/Command.java

@@ -23,6 +23,7 @@
 import java.util.function.Supplier;
 
 import org.strongback.Strongback;
+import org.strongback.control.PIDController;
 import org.strongback.util.Collections;
 
 /**
@@ -148,6 +149,33 @@ final boolean isInterruptible() {
         return interruptible;
     }
 
+    /**
+     * Create a command that uses the supplied PID controller to move within the specified tolerance of the specified setpoint.
+     *
+     * @param controller the PID+FF controller; may not be null
+     * @param setpoint the desired value for the input to the controller
+     * @param tolerance the absolute tolerance for how close the controller should come before completing the command
+     * @return the command; never null
+     */
+    public static Command use(PIDController controller, double setpoint, double tolerance) {
+        return new ControllerCommand(controller, setpoint, tolerance, controller);
+    }
+
+    /**
+     * Create a command that uses the supplied PID controller to move within the specified tolerance of the specified setpoint,
+     * timing out if the command takes longer than {@code durationInSeconds}.
+     *
+     * @param durationInSeconds the maximum duration in seconds that the command should execute; must be non-negative, and 0.0
+     *        equates to forever
+     * @param controller the PID+FF controller; may not be null
+     * @param setpoint the desired value for the input to the controller
+     * @param tolerance the absolute tolerance for how close the controller should come before completing the command
+     * @return the command; never null
+     */
+    public static Command use(double durationInSeconds, PIDController controller, double setpoint, double tolerance) {
+        return new ControllerCommand(durationInSeconds, controller, setpoint, tolerance, controller);
+    }
+
     /**
      * Create a new command object that will cancel all currently-running commands that require the supplied {@link Requirable}
      * objects. When this command is {@link Strongback#submit(Command) submitted}, it will preempt any running (or scheduled)
@@ -222,18 +250,21 @@ public static Command create(double durationInSeconds, Runnable executeFunction)
     public static Command create(double durationInSeconds, Runnable executeFunction, Runnable endFunction) {
         return new Command(durationInSeconds) {
             boolean completed = false;
+
             @Override
             public boolean execute() {
-                if ( !completed ) {
+                if (!completed) {
                     executeFunction.run();
                     completed = true;
                 }
                 return true;
             }
+
             @Override
             public void end() {
-                if ( endFunction != null ) endFunction.run();
+                if (endFunction != null) endFunction.run();
             }
+
             @Override
             public String toString() {
                 return "one-time, duration=" + durationInSeconds + " sec) " + executeFunction;
@@ -302,13 +333,12 @@ public static Command create(double timeoutInSeconds, BooleanSupplier executeFun
      *
      * @param timeoutInSeconds the time in seconds
      * @param executeFunction the function to be called during execution; may not be null
-     * @param endFunction the function to be called when the command terminates; may be null
      * @param toString the function to be called when {@link Command#toString()} is called; may not be null
      * @param requirements the {@link Requirable}s for the command
      * @return the new command; never null
      */
-    protected static Command create(double timeoutInSeconds, BooleanSupplier executeFunction,
-            Supplier<String> toString, Requirable... requirements) {
+    protected static Command create(double timeoutInSeconds, BooleanSupplier executeFunction, Supplier<String> toString,
+            Requirable... requirements) {
         return create(timeoutInSeconds, executeFunction, null, toString, requirements);
     }