diff --git a/strongback-tests/src/org/strongback/control/PIDControllerTest.java b/strongback-tests/src/org/strongback/control/PIDControllerTest.java new file mode 100644 index 0000000..70f09c6 --- /dev/null +++ b/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; + } + +} diff --git a/strongback/src/org/strongback/DataRecordable.java b/strongback/src/org/strongback/DataRecordable.java new file mode 100644 index 0000000..7e370ef --- /dev/null +++ b/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); + +} diff --git a/strongback/src/org/strongback/DataRecorder.java b/strongback/src/org/strongback/DataRecorder.java index 4d0aded..497365b 100644 --- a/strongback/src/org/strongback/DataRecorder.java +++ b/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); } diff --git a/strongback/src/org/strongback/DataRecorderChannels.java b/strongback/src/org/strongback/DataRecorderChannels.java index 687ffdc..70d6624 100644 --- a/strongback/src/org/strongback/DataRecorderChannels.java +++ b/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 iterator() { return Iterators.immutable(channels); diff --git a/strongback/src/org/strongback/command/Command.java b/strongback/src/org/strongback/command/Command.java index 94c22c5..44367df 100644 --- a/strongback/src/org/strongback/command/Command.java +++ b/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 toString, Requirable... requirements) { + protected static Command create(double timeoutInSeconds, BooleanSupplier executeFunction, Supplier toString, + Requirable... requirements) { return create(timeoutInSeconds, executeFunction, null, toString, requirements); } diff --git a/strongback/src/org/strongback/command/ControllerCommand.java b/strongback/src/org/strongback/command/ControllerCommand.java new file mode 100644 index 0000000..707378f --- /dev/null +++ b/strongback/src/org/strongback/command/ControllerCommand.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.command; + +import org.strongback.control.Controller; +import org.strongback.control.PIDController; + +/** + * A command that uses the supplied PID controller and moves to the specified setpoint. You can subclass this command class or + * use it as is. + * + * @author Randall Hauch + * @see Command#use(PIDController, double, double) + * @see Command#use(double, PIDController, double, double) + */ +public class ControllerCommand extends Command { + + private final Controller controller; + private final Runnable initializer; + + /** + * Create a command that uses the supplied PID controller and moves to 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 requirements the {@link Requirable}s this {@link Command} requires + */ + public ControllerCommand(Controller controller, double setpoint, Requirable... requirements) { + super(0.0, requirements); + this.controller = controller; + this.initializer = () -> controller.setpoint(setpoint).enable(); + } + + /** + * Create a command that uses the supplied PID controller and moves 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 + * @param requirements the {@link Requirable}s this {@link Command} requires + */ + public ControllerCommand(Controller controller, double setpoint, double tolerance, Requirable... requirements) { + super(0.0, requirements); + this.controller = controller; + this.initializer = () -> controller.setpoint(setpoint).tolerance(tolerance).enable(); + } + + /** + * Create a command that uses the supplied PID controller and moves to the specified setpoint. + * + * @param timeoutInSeconds how long in seconds this command executes before terminating, zero is forever + * @param controller the PID+FF controller; may not be null + * @param setpoint the desired value for the input to the controller + * @param requirements the {@link Requirable}s this {@link Command} requires + */ + public ControllerCommand(double timeoutInSeconds, Controller controller, double setpoint, Requirable... requirements) { + super(timeoutInSeconds, requirements); + this.controller = controller; + this.initializer = () -> controller.setpoint(setpoint).enable(); + } + + /** + * Create a command that uses the supplied PID controller and moves within the specified tolerance of the specified + * setpoint. + * + * @param timeoutInSeconds how long in seconds this command executes before terminating, zero is 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 + * @param requirements the {@link Requirable}s this {@link Command} requires + */ + public ControllerCommand(double timeoutInSeconds, Controller controller, double setpoint, double tolerance, + Requirable... requirements) { + super(timeoutInSeconds, requirements); + this.controller = controller; + this.initializer = () -> controller.setpoint(setpoint).tolerance(tolerance).enable(); + } + + @Override + public void initialize() { + super.initialize(); + if (initializer != null) initializer.run(); + } + + @Override + public boolean execute() { + return controller.computeOutput(); + } + + @Override + public void end() { + super.end(); + controller.disable(); + } + +} diff --git a/strongback/src/org/strongback/control/Controller.java b/strongback/src/org/strongback/control/Controller.java new file mode 100644 index 0000000..ef170d8 --- /dev/null +++ b/strongback/src/org/strongback/control/Controller.java @@ -0,0 +1,90 @@ +/* + * 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; + +/** + * A basic controller. + * + * @author Randall Hauch + */ +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. + * + * @return true if the execution completed because it resulted in a value that is within the tolerance of the + * setpoint, or false if this controller is not {@link #isEnabled() enabled} or has not yet reached the + * setpoint given the tolerance + */ + public boolean computeOutput(); + + /** + * Determines whether the supplied value is within the tolerance of the setpoint. + * + * @param value the proposed value + * @return true if the proposed value is within the tolerance of the setpoint, or false otherwise + */ + public boolean checkTolerance(double value); + + /** + * 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 + */ + public Controller setpoint(double setpoint); + + /** + * Sets the absolute tolerance for this controller. + * + * @param tolerance the maximum absolute error which is tolerable in the units of the input object + * @return this object so that methods can be chained; never null + */ + public Controller tolerance(double tolerance); + + /** + * Reset any error values stored from previous {@link #computeOutput() executions}. + * + * @return this object so that methods can be chained; never null + */ + public Controller reset(); + + /** + * Determine if this controller is {@link #enable() enabled}. + * + * @return true if enabled, or false otherwise + */ + public boolean isEnabled(); + + /** + * Enable this controller so that it does 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 enable(); + + /** + * Disable this controller to not 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(); + +} diff --git a/strongback/src/org/strongback/control/PIDController.java b/strongback/src/org/strongback/control/PIDController.java new file mode 100644 index 0000000..2667166 --- /dev/null +++ b/strongback/src/org/strongback/control/PIDController.java @@ -0,0 +1,574 @@ +/* + * 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 java.util.concurrent.atomic.AtomicBoolean; +import java.util.function.Consumer; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; + +import org.strongback.DataRecordable; +import org.strongback.DataRecorder; +import org.strongback.Executable; +import org.strongback.Executor; +import org.strongback.Strongback; +import org.strongback.annotation.Immutable; +import org.strongback.annotation.ThreadSafe; +import org.strongback.command.Requirable; + +import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.tables.ITable; +import edu.wpi.first.wpilibj.tables.ITableListener; + +/** + * A Proportional Integral Differential (PID) controller with optional support for feed forward. The source of inputs and the + * output to which results are written are both supplied as functions. + *

+ * There are two ways to use this controller: + *

    + *
  1. With commands - a controller instance is used within or as a subsystem, and one or more command classes are created to + * use this by setting the {@link #setpoint(double) setpoint} (and optionally the gains, input and output ranges, and tolerance) + * and calling {@link #computeOutput()}; or
    2. + *
  2. Continuous execution - a controller instance is registered with an {@link Executor} (typically Strongback's + * {@link Strongback#executor() built-in executor}) to continuously execute and generate the output based upon the input and + * current setpoint. The setpoint, gains, input and output ranges, and tolerance can all be changed at any time.
    3. + *
+ *

Recording data

+ * It is possible to register a PIDController with a {@link DataRecordable} to record separate channels for this controller's + * instantaneous values used. To record the input, output, setpoint, and error values on separate channels, simply register + * this controller's {@link #basicChannels() simple DataRecordable}: + * 
+ * PIDController controller = ...
+ * DataRecorder recorder = ...
+ * recorder.register("Azimuth Control", controller.basicChannels());
+ *
+ * To record these channels plus the individual error components, register the following instead: + * 
+ * recorder.register("Azimuth Control", controller.errorChannels());
+ *
+ * Or, to record all detailed channels, register the following instead: + * 
+ * recorder.register("Azimuth Control", controller.detailedChannels());
+ *
+ * + * @author Randall Hauch + */ +@ThreadSafe +public class PIDController implements LiveWindowSendable, Requirable, Controller { + + private final DoubleSupplier source; + private final DoubleConsumer output; + private final AtomicBoolean enabled = new AtomicBoolean(true); + private volatile Target target = new Target(); + private volatile Gains gains = new Gains(0.0, 0.0, 0.0, 0.0); + private volatile double lastInput = 0.0d; + private volatile double error = 0.0d; + private volatile double totalError = 0.0d; + private volatile double prevError = 0.0d; + private volatile double result = 0.0d; + private volatile ITable table; + private final Executable executable = new Executable() { + @Override + public void execute(long timeInMillis) { + computeOutput(); + } + }; + private final ITableListener listener = new ITableListener() { + @Override + public void valueChanged(ITable table, String key, Object value, boolean isNew) { + PIDController.this.valueChanged(table, key, value, isNew); + } + }; + + /** + * Create a new PID+FF controller that uses the supplied source for inputs and sends outputs to the supplied consumer. + * Before using, be sure to set the {@link #withGains(double, double, double, double) PID and feed forward gains}, the + * {@link #inputRange(double, double) input range}, {@link #outputRange(double, double) output range}, + * {@link #setpoint(double) setpoint}, {@link #tolerance(double) tolerance}, and whether the inputs are + * {@link #continuousInputs(boolean) continuous} (e.g., they wrap around continuously). + * + * @param source the source from which the inputs are to be read; may not be null + * @param output the output to which the calculated output is to be send; may not be null + */ + public PIDController(DoubleSupplier source, DoubleConsumer output) { + if (source == null) throw new IllegalArgumentException("The source may not be null"); + if (output == null) throw new IllegalArgumentException("The output may not be null"); + this.gains = new Gains(0.0, 0.0, 0.0, 0.0); + this.source = source; + this.output = output; + } + + /** + * 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}. + *

+ * If this is used, then this same controller should never 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() { + return executable; + } + + @Override + public boolean isEnabled() { + return enabled.get(); + } + + @Override + public PIDController enable() { + enabled.set(true); + onTable(table -> table.putBoolean("enabled", true)); + return this; + } + + @Override + public PIDController disable() { + enabled.set(false); + output.accept(0.0d); + reset(); + onTable(table -> table.putBoolean("enabled", false)); + return this; + } + + @Override + public PIDController reset() { + error = 0.0; + prevError = 0.0; + result = 0.0; + totalError = 0.0; + lastInput = 0.0; + return this; + } + + /** + * Set the gains for this controller. + * + * @param p the proportional gain + * @param i the integral gain + * @param d the differential gain + * @return this object so that methods can be chained; never null + */ + public PIDController withGains(double p, double i, double d) { + return withGains(p, i, d, 0.0); + } + + /** + * Set the gains for this controller. + * + * @param p the proportional gain + * @param i the integral gain + * @param d the differential gain + * @param feedForward the feed-forward gain + * @return this object so that methods can be chained; never null + */ + public PIDController withGains(double p, double i, double d, double feedForward) { + Gains gains = new Gains(p, i, d, feedForward); + this.gains = gains; + onTable(table -> { + table.putNumber("p", gains.p); + table.putNumber("i", gains.i); + table.putNumber("d", gains.d); + table.putNumber("f", gains.feedForward); + }); + return this; + } + + /** + * Sets whether the input range is continuous, meaning it wraps around such as with an encoder. + * + * @param continuous true if the input values are continuous values, and if the controller can take into + * account the error between the actual source and the input values; or false if the controller should + * calculate the shortest route + * @return this object so that methods can be chained; never null + * @see #inputRange(double, double) + */ + public PIDController continuousInputs(boolean continuous) { + target = target.continuous(continuous); + return this; + } + + /** + * Sets the maximum and minimum values expected from the input and setpoint. + * + * @param minimumInput the minimum value expected from the input + * @param maximumInput the maximum value expected from the input + * @return this object so that methods can be chained; never null + * @throws IllegalArgumentException if the minimum is greater than the maximum + * @see #setpoint(double) + * @see #continuousInputs(boolean) + */ + public PIDController inputRange(double minimumInput, double maximumInput) { + if (minimumInput > maximumInput) { + throw new IllegalArgumentException("Lower bound is greater than upper bound"); + } + Target target = this.target.withInputRange(minimumInput, maximumInput); // recalculates setpoint within range + this.target = target; + updateSetpoint(target); + return this; + } + + /** + * Sets the target value for this controller. The input range should be set prior to this call, and the setpoint value + * should be within the {@link #inputRange(double, double) input range} values; otherwise, the setpoint will be capped to be + * within the input range. + * + * @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 #inputRange(double, double) + */ + @Override + public PIDController setpoint(double setpoint) { + Target target = this.target.withSetpoint(setpoint); + this.target = target; + updateSetpoint(target); + return this; + } + + /** + * Sets the absolute tolerance for this controller. + * + * @param tolerance the maximum absolute error which is tolerable in the units of the input object + * @return this object so that methods can be chained; never null + * @see #inputRange(double, double) + */ + @Override + public PIDController tolerance(double tolerance) { + target = target.withTolerance(Math.abs(tolerance)); + return this; + } + + private void updateSetpoint(Target target) { + reset(); + onTable(table -> table.putNumber("setpoint", target.setpoint)); + } + + /** + * Sets the maximum and minimum values expected from the input and setpoint. + * + * @param minimumOutput the minimum value to be output + * @param maximumOutput the maximum value to be output + * @return this object so that methods can be chained; never null + * @throws IllegalArgumentException if the minimum is greater than the maximum + */ + public PIDController outputRange(double minimumOutput, double maximumOutput) { + if (minimumOutput > maximumOutput) { + throw new IllegalArgumentException("Lower bound is greater than upper bound"); + } + target = target.withOutputRange(minimumOutput, maximumOutput); + return this; + } + + @Override + public boolean checkTolerance(double value) { + return target.isWithinTolerance(value); + } + + @Override + public boolean computeOutput() { + if (enabled.get()) { + lastInput = source.getAsDouble(); + Target target = this.target; + Gains gains = this.gains; + + // Current error ... + prevError = error; + error = target.calculateError(lastInput); + + // Total error will be used in the integral term ... + if (gains.i != 0) { + double potentialIGain = (totalError + error) * gains.i; + if (potentialIGain < target.maxOutput) { + if (potentialIGain > target.minOutput) { + totalError += error; + } else { + totalError = target.minOutput / gains.i; + } + } else { + totalError = target.maxOutput / gains.i; + } + } else { + totalError = 0.0; + } + + // Calculate the new result based upon PID+FF ... + result = (gains.p * error) + (gains.i * totalError) + (gains.d * (error - prevError)) + + (target.setpoint * gains.feedForward); + + // Limit the results ... + result = target.limitOutput(result); + + // Output the result ... + output.accept(result); + + // Determine if we're within the tolerance ... + return Math.abs(error) < target.tolerance; + } + return false; + } + + /** + * Get a {@link DataRecordable recordable} object that can be used to {@link DataRecorder#register(String, DataRecordable) + * register} this controller's input, output, setpoint, and error values as separate channels. + * + * @return the recordable object; never null + * @see #errorChannels() + * @see #detailedChannels() + */ + public DataRecordable basicChannels() { + return (recorder, name) -> { + recorder.register(name + " input", () -> lastInput); + recorder.register(name + " output", () -> result); + recorder.register(name + " setpoint", () -> target.setpoint); + recorder.register(name + " error", () -> error); + }; + } + + /** + * Get a {@link DataRecordable recordable} object that can be used to {@link DataRecorder#register(String, DataRecordable) + * register} multiple channels for this controller. The channels include: + *

    + *
  • input values
    • + *
  • output values
    • + *
  • setpoint values
    • + *
  • error values
    • + *
  • proportional error values
    • + *
  • integral error values
    • + *
  • differential error values
    • + *
  • feed forward error values
    • + *
+ * + * @return the recordable object; never null + * @see #basicChannels() + * @see #detailedChannels() + */ + public DataRecordable errorChannels() { + return (recorder, name) -> { + basicChannels().registerWith(recorder, name); + recorder.register(name + " error(P)", () -> gains.p * error); + recorder.register(name + " error(I)", () -> gains.i * totalError); + recorder.register(name + " error(D)", () -> gains.d * (error - prevError)); + recorder.register(name + " error(F)", () -> target.setpoint * gains.feedForward); + }; + } + + /** + * Get a {@link DataRecordable recordable} object that can be used to {@link DataRecorder#register(String, DataRecordable) + * register} multiple channels for this controller. The channels include: + *
    + *
  • input values
    • + *
  • output values
    • + *
  • setpoint values
    • + *
  • tolerance values
    • + *
  • error values
    • + *
  • proportional error values
    • + *
  • integral error values
    • + *
  • differential error values
    • + *
  • feed forward error values
    • + *
  • total (accumulated) error value
    • + *
  • proportional gain
    • + *
  • integral gain
    • + *
  • differential gain
    • + *
  • feed forward gain
    • + *
+ * + * @return the recordable object; never null + * @see #basicChannels() + * @see #basicChannels() + */ + public DataRecordable detailedChannels() { + return (recorder, name) -> { + errorChannels().registerWith(recorder, name); + recorder.register(name + " tolerance", () -> target.tolerance); + recorder.register(name + " gain(P)", () -> gains.p); + recorder.register(name + " gain(I)", () -> gains.i); + recorder.register(name + " gain(D)", () -> gains.d); + recorder.register(name + " gain(F)", () -> gains.feedForward); + recorder.register(name + " error total", () -> totalError); + }; + } + + protected static final class Gains { + protected final double p; + protected final double i; + protected final double d; + protected final double feedForward; + + public Gains(double p, double i, double d, double feedForward) { + this.p = p; + this.i = i; + this.d = d; + this.feedForward = feedForward; + }; + } + + @Immutable + protected static final class Target { + protected final double maxOutput; + protected final double minOutput; + protected final double maxInput; + protected final double minInput; + protected final double setpoint; + protected final double tolerance; + protected final boolean continuous; + + public Target() { + this(0.0, 0.0, 0.0, 0.001, true, -1.0, 1.0); + } + + public Target(double setpoint, double minInput, double maxInput, double tolerance, boolean continuous, double minOutput, + double maxOutput) { + this.minInput = minInput; + this.maxInput = maxInput; + this.tolerance = tolerance; + this.minOutput = minOutput; + this.maxOutput = maxOutput; + this.continuous = continuous; + if (maxInput > minInput) { + if (setpoint > maxInput) { + this.setpoint = maxInput; + } else if (setpoint < minInput) { + this.setpoint = minInput; + } else { + this.setpoint = setpoint; + } + } else { + this.setpoint = setpoint; + } + } + + public Target withInputRange(double minInput, double maxInput) { + return new Target(setpoint, minInput, maxInput, tolerance, continuous, minOutput, maxOutput); + } + + public Target withOutputRange(double minOutput, double maxOutput) { + return new Target(setpoint, minInput, maxInput, tolerance, continuous, minOutput, maxOutput); + } + + public Target withSetpoint(double setpoint) { + return new Target(setpoint, minInput, maxInput, tolerance, continuous, minOutput, maxOutput); + } + + public Target withTolerance(double tolerance) { + return new Target(setpoint, minInput, maxInput, tolerance, continuous, minOutput, maxOutput); + } + + public Target continuous(boolean continuous) { + return new Target(setpoint, minInput, maxInput, tolerance, continuous, minOutput, maxOutput); + } + + public boolean isWithinTolerance(double value) { + return Math.abs(value) <= (setpoint - tolerance); + } + + public double calculateError(double input) { + double error = setpoint - input; + if (continuous) { + if (Math.abs(error) > (maxInput - minInput) / 2.0) { + if (error > 0) { + error = error - maxInput + minInput; + } else { + error = error + maxInput - minInput; + } + } + } + return error; + } + + public double limitOutput(double output) { + if (output > maxOutput) return maxOutput; + if (output < minOutput) return minOutput; + return output; + } + } + + protected void onTable(Consumer updateTable) { + ITable table = this.table; + if (table != null) updateTable.accept(table); + } + + protected void valueChanged(ITable table, String key, Object value, boolean isNew) { + if (key.equals("p") || key.equals("i") || key.equals("d") || key.equals("f")) { + Gains gains = this.gains; + if (gains.p != table.getNumber("p", 0.0) || gains.i != table.getNumber("i", 0.0) + || gains.d != table.getNumber("d", 0.0) || gains.feedForward != table.getNumber("f", 0.0)) { + withGains(table.getNumber("p", 0.0), + table.getNumber("i", 0.0), + table.getNumber("d", 0.0), + table.getNumber("f", 0.0)); + } + } else if (key.equals("setpoint")) { + Target target = this.target; + if (target.setpoint != ((Double) value).doubleValue()) { + setpoint(((Double) value).doubleValue()); + } + } else if (key.equals("enabled")) { + if (isEnabled() != ((Boolean) value).booleanValue()) { + if (((Boolean) value).booleanValue()) { + enable(); + } else { + disable(); + } + } + } + } + + @Override + public synchronized void initTable(ITable subtable) { + if (this.table != null) { + this.table.removeTableListener(listener); + } + this.table = table; + if (table != null) { + Gains gains = this.gains; + Target target = this.target; + table.putNumber("p", gains.p); + table.putNumber("i", gains.i); + table.putNumber("d", gains.d); + table.putNumber("f", gains.feedForward); + table.putNumber("setpoint", target.setpoint); + table.putBoolean("enabled", isEnabled()); + table.addTableListener(listener, false); + } + } + + @Override + public String getSmartDashboardType() { + return "PIDController"; + } + + @Override + public ITable getTable() { + return table; + } + + @Override + public void startLiveWindowMode() { + disable(); + } + + @Override + public void stopLiveWindowMode() { + } + + @Override + public void updateTable() { + } + +}