From 222669dc2c6e159ff2fc3adc8dbc15ce9e06dd11 Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 1 Jan 2020 15:23:25 -0800 Subject: [PATCH] Fix trapezoidal profile PID controller setpoint bug (#2210) Co-Authored-By: Austin Shalit --- .../wpilibj2/command/ProfiledPIDCommand.java | 2 +- .../command/ProfiledPIDSubsystem.java | 2 +- .../include/frc2/command/ProfiledPIDCommand.h | 2 +- .../frc2/command/ProfiledPIDSubsystem.h | 4 +-- .../frc/controller/ProfiledPIDController.h | 34 ++++++++++++++++--- .../wpilibj/controller/PIDController.java | 2 +- .../controller/ProfiledPIDController.java | 32 ++++++++++++++--- .../controller/ProfiledPIDControllerTest.java | 26 ++++++++++++++ 8 files changed, 90 insertions(+), 14 deletions(-) create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java index 076b4b89cd..aa375f1499 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java @@ -113,7 +113,7 @@ public class ProfiledPIDCommand extends CommandBase { @Override public void initialize() { - m_controller.reset(); + m_controller.reset(m_measurement.getAsDouble()); } @Override diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java index 07283c504a..06a8699638 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java @@ -93,7 +93,7 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase { */ public void enable() { m_enabled = true; - m_controller.reset(); + m_controller.reset(getMeasurement()); } /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h index 49575de988..a1ab1b80e2 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h @@ -118,7 +118,7 @@ class ProfiledPIDCommand ProfiledPIDCommand(const ProfiledPIDCommand& other) = default; - void Initialize() override { m_controller.Reset(); } + void Initialize() override { m_controller.Reset(m_measurement()); } void Execute() override { m_useOutput(m_controller.Calculate(m_measurement(), m_goal()), diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h index 8d1a618e56..d571f1a15e 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h @@ -62,10 +62,10 @@ class ProfiledPIDSubsystem : public SubsystemBase { void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; } /** - * Enables the PID control. Resets the controller. + * Enables the PID control. Resets the controller. */ virtual void Enable() { - m_controller.Reset(); + m_controller.Reset(GetMeasurement()); m_enabled = true; } diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h index 52e2fce481..079d96eda1 100644 --- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 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. */ @@ -46,7 +46,8 @@ class ProfiledPIDController public: /** * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and - * Kd. + * Kd. Users should call reset() when they first start running the controller + * to avoid unwanted behavior. * * @param Kp The proportional coefficient. * @param Ki The integral coefficient. @@ -292,9 +293,34 @@ class ProfiledPIDController } /** - * Reset the previous error, the integral term, and disable the controller. + * Reset the previous error and the integral term. + * + * @param measurement The current measured State of the system. */ - void Reset() { m_controller.Reset(); } + void Reset(const State& measurement) { + m_controller.Reset(); + m_setpoint = measurement; + } + + /** + * Reset the previous error and the integral term. + * + * @param measuredPosition The current measured position of the system. + * @param measuredVelocity The current measured velocity of the system. + */ + void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) { + Reset(State{measuredPosition, measuredVelocity}); + } + + /** + * Reset the previous error and the integral term. + * + * @param measuredPosition The current measured position of the system. The + * velocity is assumed to be zero. + */ + void Reset(Distance_t measuredPosition) { + Reset(measuredPosition, Velocity_t(0)); + } void InitSendable(frc::SendableBuilder& builder) override { builder.SetSmartDashboardType("ProfiledPIDController"); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java index 39b6a17692..e9bcc533b8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java @@ -328,7 +328,7 @@ public class PIDController implements Sendable, AutoCloseable { } /** - * Resets the previous error and the integral term. Also disables the controller. + * Resets the previous error and the integral term. */ public void reset() { m_prevError = 0; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java index edbae26d11..7550aa58cc 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 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. */ @@ -15,7 +15,8 @@ import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; /** * Implements a PID control loop whose setpoint is constrained by a trapezoid - * profile. + * profile. Users should call reset() when they first start running the controller + * to avoid unwanted behavior. */ @SuppressWarnings("PMD.TooManyMethods") public class ProfiledPIDController implements Sendable { @@ -319,10 +320,33 @@ public class ProfiledPIDController implements Sendable { } /** - * Reset the previous error, the integral term, and disable the controller. + * Reset the previous error and the integral term. + * + * @param measurement The current measured State of the system. */ - public void reset() { + public void reset(TrapezoidProfile.State measurement) { m_controller.reset(); + m_setpoint = measurement; + } + + /** + * Reset the previous error and the integral term. + * + * @param measuredPosition The current measured position of the system. + * @param measuredVelocity The current measured velocity of the system. + */ + public void reset(double measuredPosition, double measuredVelocity) { + reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity)); + } + + /** + * Reset the previous error and the integral term. + * + * @param measuredPosition The current measured position of the system. The velocity is + * assumed to be zero. + */ + public void reset(double measuredPosition) { + reset(measuredPosition, 0.0); } @Override diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java new file mode 100644 index 0000000000..21cec87355 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +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); + } +}