[wpimath] Move controller from wpilibj to wpimath (#3439)

wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java

@@ -0,0 +1,89 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import java.util.ArrayList;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class HolonomicDriveControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  @Test
+  void testReachesReference() {
+    HolonomicDriveController controller =
+        new HolonomicDriveController(
+            new PIDController(1.0, 0.0, 0.0),
+            new PIDController(1.0, 0.0, 0.0),
+            new ProfiledPIDController(1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(6.28, 3.14)));
+    Pose2d robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+    List<Pose2d> waypoints = new ArrayList<>();
+    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.8)));
+
+    TrajectoryConfig config = new TrajectoryConfig(8.0, 4.0);
+    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final double kDt = 0.02;
+    final double kTotalTime = trajectory.getTotalTimeSeconds();
+
+    for (int i = 0; i < (kTotalTime / kDt); i++) {
+      Trajectory.State state = trajectory.sample(kDt * i);
+      ChassisSpeeds output = controller.calculate(robotPose, state, new Rotation2d());
+
+      robotPose =
+          robotPose.exp(
+              new Twist2d(
+                  output.vxMetersPerSecond * kDt,
+                  output.vyMetersPerSecond * kDt,
+                  output.omegaRadiansPerSecond * kDt));
+    }
+
+    final List<Trajectory.State> states = trajectory.getStates();
+    final Pose2d endPose = states.get(states.size() - 1).poseMeters;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final Pose2d finalRobotPose = robotPose;
+
+    assertAll(
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+        () ->
+            assertEquals(
+                0.0,
+                MathUtil.angleModulus(finalRobotPose.getRotation().getRadians()),
+                kAngularTolerance));
+  }
+
+  @Test
+  void testDoesNotRotateUnnecessarily() {
+    var controller =
+        new HolonomicDriveController(
+            new PIDController(1, 0, 0),
+            new PIDController(1, 0, 0),
+            new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(4, 2)));
+
+    ChassisSpeeds speeds =
+        controller.calculate(
+            new Pose2d(0, 0, new Rotation2d(1.57)), new Pose2d(), 0, new Rotation2d(1.57));
+
+    assertEquals(0.0, speeds.omegaRadiansPerSecond);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java

@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class PIDInputOutputTest {
+  private PIDController m_controller;
+
+  @BeforeEach
+  void setUp() {
+    m_controller = new PIDController(0, 0, 0);
+  }
+
+  @Test
+  void continuousInputTest() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-180, 180);
+    assertEquals(m_controller.calculate(-179, 179), -2, 1e-5);
+
+    m_controller.enableContinuousInput(0, 360);
+    assertEquals(m_controller.calculate(1, 359), -2, 1e-5);
+  }
+
+  @Test
+  void proportionalGainOutputTest() {
+    m_controller.setP(4);
+
+    assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5);
+  }
+
+  @Test
+  void integralGainOutputTest() {
+    m_controller.setI(4);
+
+    double out = 0;
+
+    for (int i = 0; i < 5; i++) {
+      out = m_controller.calculate(0.025, 0);
+    }
+
+    assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
+  }
+
+  @Test
+  void derivativeGainOutputTest() {
+    m_controller.setD(4);
+
+    m_controller.calculate(0, 0);
+
+    assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java

@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class PIDToleranceTest {
+  private static final double kSetpoint = 50.0;
+  private static final double kTolerance = 10.0;
+  private static final double kRange = 200;
+
+  @Test
+  void initialToleranceTest() {
+    try (var controller = new PIDController(0.05, 0.0, 0.0)) {
+      controller.enableContinuousInput(-kRange / 2, kRange / 2);
+
+      assertTrue(controller.atSetpoint());
+    }
+  }
+
+  @Test
+  void absoluteToleranceTest() {
+    try (var controller = new PIDController(0.05, 0.0, 0.0)) {
+      controller.enableContinuousInput(-kRange / 2, kRange / 2);
+
+      assertTrue(
+          controller.atSetpoint(),
+          "Error was not in tolerance when it should have been. Error was "
+              + controller.getPositionError());
+
+      controller.setTolerance(kTolerance);
+      controller.setSetpoint(kSetpoint);
+
+      assertFalse(
+          controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+              + controller.getPositionError());
+
+      controller.calculate(0.0);
+
+      assertFalse(
+          controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+              + controller.getPositionError());
+
+      controller.calculate(kSetpoint + kTolerance / 2);
+
+      assertTrue(
+          controller.atSetpoint(),
+          "Error was not in tolerance when it should have been. Error was "
+              + controller.getPositionError());
+
+      controller.calculate(kSetpoint + 10 * kTolerance);
+
+      assertFalse(
+          controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+              + controller.getPositionError());
+    }
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/ProfiledPIDControllerTest.java

@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import org.junit.jupiter.api.Test;
+
+class ProfiledPIDControllerTest {
+  @Test
+  void testStartFromNonZeroPosition() {
+    ProfiledPIDController controller =
+        new ProfiledPIDController(1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1.0, 1.0));
+
+    controller.reset(20);
+
+    assertEquals(0.0, controller.calculate(20), 0.05);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/ProfiledPIDInputOutputTest.java

@@ -0,0 +1,114 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class ProfiledPIDInputOutputTest {
+  private ProfiledPIDController m_controller;
+
+  @BeforeEach
+  void setUp() {
+    m_controller = new ProfiledPIDController(0, 0, 0, new TrapezoidProfile.Constraints(360, 180));
+  }
+
+  @Test
+  void continuousInputTest1() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-180, 180);
+
+    final double kSetpoint = -179.0;
+    final double kMeasurement = -179.0;
+    final double kGoal = 179.0;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < 180.0);
+  }
+
+  @Test
+  void continuousInputTest2() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.4826633343199735;
+    final double kMeasurement = -3.1352207333939606;
+    final double kGoal = -3.534162788601621;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void continuousInputTest3() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.5176604690006377;
+    final double kMeasurement = 3.1191729343822456;
+    final double kGoal = 2.709680418117445;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void continuousInputTest4() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(0, 2.0 * Math.PI);
+
+    final double kSetpoint = 2.78;
+    final double kMeasurement = 3.12;
+    final double kGoal = 2.71;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI / 2.0);
+  }
+
+  @Test
+  void proportionalGainOutputTest() {
+    m_controller.setP(4);
+
+    assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5);
+  }
+
+  @Test
+  void integralGainOutputTest() {
+    m_controller.setI(4);
+
+    double out = 0;
+
+    for (int i = 0; i < 5; i++) {
+      out = m_controller.calculate(0.025, 0);
+    }
+
+    assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
+  }
+
+  @Test
+  void derivativeGainOutputTest() {
+    m_controller.setD(4);
+
+    m_controller.calculate(0, 0);
+
+    assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
+  }
+}