Add replacement PIDController class (#1300)

Originally, PIDController used PIDSource with its "PIDSourceType" to
determine whether a class should return position or velocity to the
controller. However, the supported languages have changed a lot over 10
years and now support lambdas. Instead of using PIDSource and PIDOutput,
users can pass in doubles to the Calculate() function synchronously.
This makes the controller much more flexible for team's needs as they no
longer have to make a separate PIDSource-inheriting class just to
provide a custom input.

The built-in feedforward was removed. Since PIDController is synchronous
now, they can add their own feedforward on top of what Calculate()
returns.

To facilitate running the controller asynchronously, there is a
PIDControllerRunner class that handles that. By separating the loop from
the control law, PIDController can now be composed with others and be
used to control a drivetrain (a multiple input, multiple output system
that requires summing the results from two controllers) much easier.
Also, motion profiling can be used to set the reference over time.

All the classes related to the old PIDController are now deprecated. The
new classes are in an experimental namespace to avoid name conflicts.

While this is a large change, I think it is a necessary one for growth.
The old PIDController design was created in a time when languages only
supported OOP, and we have more tools at our disposal now to solve
problems. This more versatile implementation can be used in more places
like as a replacement for Pathfinder's "EncoderFollower" class.

There has been hesitation to add lambda support to WPILib for a while
now out of concerns for requiring teams to learn more features of C++ or
Java. In my opinion, this change makes PIDController easier to use, not
harder. The concept of a function is a building block of OOP and should
be learned before classes. The ability to store functions as first-class
objects and invoke them just like variables is rather natural.

Note that PID constants for the new controller will be different from
the old one. The original controller didn't take the discretization
period into account. To fix this, teams should just have to divide their
Ki gain by 0.05 and multiply their Kd gain by 0.05 where 0.05 is the
original default period.

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,6 +19,8 @@ import org.junit.runner.RunWith;
 import org.junit.runners.Parameterized;
 import org.junit.runners.Parameterized.Parameters;
 
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.PIDControllerRunner;
 import edu.wpi.first.wpilibj.filters.LinearDigitalFilter;
 import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
@@ -175,42 +177,44 @@ public class MotorEncoderTest extends AbstractComsSetup {
 
   @Test
   public void testPositionPIDController() {
-    me.getEncoder().setPIDSourceType(PIDSourceType.kDisplacement);
-    PIDController pid = new PIDController(0.001, 0.0005, 0, me.getEncoder(), me.getMotor());
-    pid.setAbsoluteTolerance(50.0);
-    pid.setOutputRange(-0.2, 0.2);
-    pid.setSetpoint(1000);
+    PIDController pidController = new PIDController(0.001, 0.0005, 0);
+    pidController.setAbsoluteTolerance(50.0);
+    pidController.setOutputRange(-0.2, 0.2);
+    pidController.setSetpoint(1000);
 
-    pid.enable();
+    PIDControllerRunner pidRunner = new PIDControllerRunner(pidController,
+        me.getEncoder()::getDistance, output -> me.getMotor().set(output));
+    pidRunner.enable();
     Timer.delay(10.0);
-    pid.disable();
+    pidRunner.disable();
 
     assertTrue(
-        "PID loop did not reach setpoint within 10 seconds. The current error was" + pid
-            .getError(), pid.onTarget());
+        "PID loop did not reach reference within 10 seconds. The current error was" + pidController
+            .getError(), pidController.atSetpoint());
 
-    pid.close();
+    pidController.close();
   }
 
   @Test
   public void testVelocityPIDController() {
     me.getEncoder().setPIDSourceType(PIDSourceType.kRate);
     LinearDigitalFilter filter = LinearDigitalFilter.movingAverage(me.getEncoder(), 50);
-    PIDController pid =
-        new PIDController(1e-5, 0.0, 3e-5, 8e-5, filter, me.getMotor());
-    pid.setAbsoluteTolerance(200);
-    pid.setOutputRange(-0.3, 0.3);
-    pid.setSetpoint(600);
+    PIDController pidController = new PIDController(1e-5, 0.0, 0.0006);
+    pidController.setAbsoluteTolerance(200);
+    pidController.setOutputRange(-0.3, 0.3);
+    pidController.setSetpoint(600);
 
-    pid.enable();
+    PIDControllerRunner pidRunner = new PIDControllerRunner(pidController, filter::pidGet,
+        output -> me.getMotor().set(output + 8e-5));
+    pidRunner.enable();
     Timer.delay(10.0);
-    pid.disable();
+    pidRunner.disable();
 
     assertTrue(
-        "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getError(),
-        pid.onTarget());
+        "PID loop did not reach reference within 10 seconds. The error was: "
+        + pidController.getError(), pidController.atSetpoint());
 
-    pid.close();
+    pidController.close();
   }
 
   /**

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -23,6 +23,8 @@ import org.junit.runners.Parameterized.Parameters;
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.PIDControllerRunner;
 import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
@@ -50,6 +52,7 @@ public class PIDTest extends AbstractComsSetup {
   private static final double outputRange = 0.25;
 
   private PIDController m_controller = null;
+  private PIDControllerRunner m_runner = null;
   private static MotorEncoderFixture me = null;
 
   @SuppressWarnings({"MemberName", "EmptyLineSeparator", "MultipleVariableDeclarations"})
@@ -108,16 +111,19 @@ public class PIDTest extends AbstractComsSetup {
     m_table = NetworkTableInstance.getDefault().getTable("TEST_PID");
     m_builder = new SendableBuilderImpl();
     m_builder.setTable(m_table);
-    m_controller = new PIDController(k_p, k_i, k_d, me.getEncoder(), me.getMotor());
+    m_controller = new PIDController(k_p, k_i, k_d);
+    m_runner = new PIDControllerRunner(m_controller,
+        me.getEncoder()::getDistance, me.getMotor()::set);
     m_controller.initSendable(m_builder);
   }
 
   @After
   public void tearDown() throws Exception {
     logger.fine("Teardown: " + me.getType());
-    m_controller.disable();
+    m_runner.disable();
     m_controller.close();
     m_controller = null;
+    m_runner = null;
     me.reset();
   }
 
@@ -133,16 +139,16 @@ public class PIDTest extends AbstractComsSetup {
   public void testInitialSettings() {
     setupAbsoluteTolerance();
     setupOutputRange();
-    double setpoint = 2500.0;
-    m_controller.setSetpoint(setpoint);
-    assertFalse("PID did not begin disabled", m_controller.isEnabled());
-    assertEquals("PID.getError() did not start at " + setpoint, setpoint,
+    double reference = 2500.0;
+    m_controller.setSetpoint(reference);
+    assertFalse("PID did not begin disabled", m_runner.isEnabled());
+    assertEquals("PID.getError() did not start at " + reference, reference,
         m_controller.getError(), 0);
     m_builder.updateTable();
-    assertEquals(k_p, m_table.getEntry("p").getDouble(9999999), 0);
-    assertEquals(k_i, m_table.getEntry("i").getDouble(9999999), 0);
-    assertEquals(k_d, m_table.getEntry("d").getDouble(9999999), 0);
-    assertEquals(setpoint, m_table.getEntry("setpoint").getDouble(9999999), 0);
+    assertEquals(k_p, m_table.getEntry("Kp").getDouble(9999999), 0);
+    assertEquals(k_i, m_table.getEntry("Ki").getDouble(9999999), 0);
+    assertEquals(k_d, m_table.getEntry("Kd").getDouble(9999999), 0);
+    assertEquals(reference, m_table.getEntry("reference").getDouble(9999999), 0);
     assertFalse(m_table.getEntry("enabled").getBoolean(true));
   }
 
@@ -150,17 +156,17 @@ public class PIDTest extends AbstractComsSetup {
   public void testRestartAfterEnable() {
     setupAbsoluteTolerance();
     setupOutputRange();
-    double setpoint = 2500.0;
-    m_controller.setSetpoint(setpoint);
-    m_controller.enable();
+    double reference = 2500.0;
+    m_controller.setSetpoint(reference);
+    m_runner.enable();
     m_builder.updateTable();
     assertTrue(m_table.getEntry("enabled").getBoolean(false));
-    assertTrue(m_controller.isEnabled());
+    assertTrue(m_runner.isEnabled());
     assertThat(0.0, is(not(me.getMotor().get())));
     m_controller.reset();
     m_builder.updateTable();
     assertFalse(m_table.getEntry("enabled").getBoolean(true));
-    assertFalse(m_controller.isEnabled());
+    assertFalse(m_runner.isEnabled());
     assertEquals(0, me.getMotor().get(), 0);
   }
 
@@ -168,11 +174,11 @@ public class PIDTest extends AbstractComsSetup {
   public void testSetSetpoint() {
     setupAbsoluteTolerance();
     setupOutputRange();
-    Double setpoint = 2500.0;
-    m_controller.disable();
-    m_controller.setSetpoint(setpoint);
-    m_controller.enable();
-    assertEquals("Did not correctly set set-point", setpoint, new Double(m_controller
+    Double reference = 2500.0;
+    m_runner.disable();
+    m_controller.setSetpoint(reference);
+    m_runner.enable();
+    assertEquals("Did not correctly set reference", reference, new Double(m_controller
         .getSetpoint()));
   }
 
@@ -180,16 +186,16 @@ public class PIDTest extends AbstractComsSetup {
   public void testRotateToTarget() {
     setupAbsoluteTolerance();
     setupOutputRange();
-    double setpoint = 1000.0;
-    assertEquals(pidData() + "did not start at 0", 0, m_controller.get(), 0);
-    m_controller.setSetpoint(setpoint);
-    assertEquals(pidData() + "did not have an error of " + setpoint, setpoint,
+    double reference = 1000.0;
+    assertEquals(pidData() + "did not start at 0", 0, m_controller.getOutput(), 0);
+    m_controller.setSetpoint(reference);
+    assertEquals(pidData() + "did not have an error of " + reference, reference,
         m_controller.getError(), 0);
-    m_controller.enable();
+    m_runner.enable();
     Timer.delay(5);
-    m_controller.disable();
+    m_runner.disable();
     assertTrue(pidData() + "Was not on Target. Controller Error: " + m_controller.getError(),
-        m_controller.onTarget());
+        m_controller.atSetpoint());
   }
 
   private String pidData() {
@@ -201,6 +207,6 @@ public class PIDTest extends AbstractComsSetup {
   @Test(expected = RuntimeException.class)
   public void testOnTargetNoToleranceSet() {
     setupOutputRange();
-    m_controller.onTarget();
+    m_controller.atSetpoint();
   }
 }