diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp new file mode 100644 index 0000000000..8bbbe96d47 --- /dev/null +++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp @@ -0,0 +1,265 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/controller/PIDController.h" + +#include +#include + +#include + +#include "frc/smartdashboard/SendableBuilder.h" + +using namespace frc2; + +PIDController::PIDController(double Kp, double Ki, double Kd, double period) + : frc::SendableBase(false), m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) { + static int instances = 0; + instances++; + HAL_Report(HALUsageReporting::kResourceType_PIDController, instances); + SetName("PIDController", instances); +} + +PIDController::PIDController(PIDController&& rhs) + : SendableBase(std::move(rhs)), + m_Kp(std::move(rhs.m_Kp)), + m_Ki(std::move(rhs.m_Ki)), + m_Kd(std::move(rhs.m_Kd)), + m_period(std::move(rhs.m_period)), + m_maximumOutput(std::move(rhs.m_maximumOutput)), + m_minimumOutput(std::move(rhs.m_minimumOutput)), + m_maximumInput(std::move(rhs.m_maximumInput)), + m_minimumInput(std::move(rhs.m_minimumInput)), + m_inputRange(std::move(rhs.m_inputRange)), + m_continuous(std::move(rhs.m_continuous)), + m_currError(std::move(rhs.m_currError)), + m_prevError(std::move(rhs.m_prevError)), + m_totalError(std::move(rhs.m_totalError)), + m_toleranceType(std::move(rhs.m_toleranceType)), + m_tolerance(std::move(rhs.m_tolerance)), + m_deltaTolerance(std::move(rhs.m_deltaTolerance)), + m_setpoint(std::move(rhs.m_setpoint)), + m_output(std::move(rhs.m_output)) {} + +PIDController& PIDController::operator=(PIDController&& rhs) { + std::scoped_lock lock(m_thisMutex, rhs.m_thisMutex); + + SendableBase::operator=(std::move(rhs)); + + m_Kp = std::move(rhs.m_Kp); + m_Ki = std::move(rhs.m_Ki); + m_Kd = std::move(rhs.m_Kd); + m_period = std::move(rhs.m_period); + m_maximumOutput = std::move(rhs.m_maximumOutput); + m_minimumOutput = std::move(rhs.m_minimumOutput); + m_maximumInput = std::move(rhs.m_maximumInput); + m_minimumInput = std::move(rhs.m_minimumInput); + m_inputRange = std::move(rhs.m_inputRange); + m_continuous = std::move(rhs.m_continuous); + m_currError = std::move(rhs.m_currError); + m_prevError = std::move(rhs.m_prevError); + m_totalError = std::move(rhs.m_totalError); + m_toleranceType = std::move(rhs.m_toleranceType); + m_tolerance = std::move(rhs.m_tolerance); + m_deltaTolerance = std::move(rhs.m_deltaTolerance); + m_setpoint = std::move(rhs.m_setpoint); + m_output = std::move(rhs.m_output); + + return *this; +} + +void PIDController::SetP(double Kp) { + std::lock_guard lock(m_thisMutex); + m_Kp = Kp; +} + +void PIDController::SetI(double Ki) { + std::lock_guard lock(m_thisMutex); + m_Ki = Ki; +} + +void PIDController::SetD(double Kd) { + std::lock_guard lock(m_thisMutex); + m_Kd = Kd; +} + +double PIDController::GetP() const { + std::lock_guard lock(m_thisMutex); + return m_Kp; +} + +double PIDController::GetI() const { + std::lock_guard lock(m_thisMutex); + return m_Ki; +} + +double PIDController::GetD() const { + std::lock_guard lock(m_thisMutex); + return m_Kd; +} + +double PIDController::GetPeriod() const { return m_period; } + +double PIDController::GetOutput() const { + std::lock_guard lock(m_thisMutex); + return m_output; +} + +void PIDController::SetSetpoint(double setpoint) { + std::lock_guard lock(m_thisMutex); + + if (m_maximumInput > m_minimumInput) { + m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput); + } else { + m_setpoint = setpoint; + } +} + +double PIDController::GetSetpoint() const { + std::lock_guard lock(m_thisMutex); + return m_setpoint; +} + +bool PIDController::AtSetpoint(double tolerance, double deltaTolerance, + Tolerance toleranceType) const { + double deltaError = GetDeltaError(); + + std::lock_guard lock(m_thisMutex); + if (toleranceType == Tolerance::kPercent) { + return std::abs(m_currError) < tolerance / 100 * m_inputRange && + std::abs(deltaError) < deltaTolerance / 100 * m_inputRange; + } else { + return std::abs(m_currError) < tolerance && + std::abs(deltaError) < deltaTolerance; + } +} + +bool PIDController::AtSetpoint() const { + return AtSetpoint(m_tolerance, m_deltaTolerance, m_toleranceType); +} + +void PIDController::SetContinuous(bool continuous) { + std::lock_guard lock(m_thisMutex); + m_continuous = continuous; +} + +void PIDController::SetInputRange(double minimumInput, double maximumInput) { + std::lock_guard lock(m_thisMutex); + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; + m_inputRange = maximumInput - minimumInput; + + // Clamp setpoint to new input range + if (m_maximumInput > m_minimumInput) { + m_setpoint = std::clamp(m_setpoint, m_minimumInput, m_maximumInput); + } +} + +void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) { + std::lock_guard lock(m_thisMutex); + m_minimumOutput = minimumOutput; + m_maximumOutput = maximumOutput; +} + +void PIDController::SetAbsoluteTolerance(double tolerance, + double deltaTolerance) { + std::lock_guard lock(m_thisMutex); + m_toleranceType = Tolerance::kAbsolute; + m_tolerance = tolerance; + m_deltaTolerance = deltaTolerance; +} + +void PIDController::SetPercentTolerance(double tolerance, + double deltaTolerance) { + std::lock_guard lock(m_thisMutex); + m_toleranceType = Tolerance::kPercent; + m_tolerance = tolerance; + m_deltaTolerance = deltaTolerance; +} + +double PIDController::GetError() const { + std::lock_guard lock(m_thisMutex); + return GetContinuousError(m_currError); +} + +/** + * Returns the change in error per second of the PIDController. + * + * @return The change in error per second. + */ +double PIDController::GetDeltaError() const { + std::lock_guard lock(m_thisMutex); + return (m_currError - m_prevError) / GetPeriod(); +} + +double PIDController::Calculate(double measurement) { + std::lock_guard lock(m_thisMutex); + return CalculateUnsafe(measurement); +} + +double PIDController::Calculate(double measurement, double setpoint) { + std::lock_guard lock(m_thisMutex); + + // Set setpoint to provided value + if (m_maximumInput > m_minimumInput) { + m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput); + } else { + m_setpoint = setpoint; + } + + return CalculateUnsafe(measurement); +} + +void PIDController::Reset() { + std::lock_guard lock(m_thisMutex); + m_prevError = 0; + m_totalError = 0; + m_output = 0; +} + +void PIDController::InitSendable(frc::SendableBuilder& builder) { + builder.SetSmartDashboardType("PIDController"); + builder.AddDoubleProperty("p", [this] { return GetP(); }, + [this](double value) { SetP(value); }); + builder.AddDoubleProperty("i", [this] { return GetI(); }, + [this](double value) { SetI(value); }); + builder.AddDoubleProperty("d", [this] { return GetD(); }, + [this](double value) { SetD(value); }); + builder.AddDoubleProperty("setpoint", [this] { return GetSetpoint(); }, + [this](double value) { SetSetpoint(value); }); +} + +double PIDController::GetContinuousError(double error) const { + if (m_continuous && m_inputRange > 0) { + error = std::fmod(error, m_inputRange); + if (std::abs(error) > m_inputRange / 2) { + if (error > 0) { + return error - m_inputRange; + } else { + return error + m_inputRange; + } + } + } + + return error; +} + +double PIDController::CalculateUnsafe(double measurement) { + m_prevError = m_currError; + m_currError = GetContinuousError(m_setpoint - measurement); + + if (m_Ki != 0) { + m_totalError = std::clamp(m_totalError + m_currError * GetPeriod(), + m_minimumOutput / m_Ki, m_maximumOutput / m_Ki); + } + + m_output = std::clamp(m_Kp * m_currError + m_Ki * m_totalError + + m_Kd * (m_currError - m_prevError) / GetPeriod(), + m_minimumOutput, m_maximumOutput); + + return m_output; +} diff --git a/wpilibc/src/main/native/cpp/controller/PIDControllerRunner.cpp b/wpilibc/src/main/native/cpp/controller/PIDControllerRunner.cpp new file mode 100644 index 0000000000..5f7f78a9a4 --- /dev/null +++ b/wpilibc/src/main/native/cpp/controller/PIDControllerRunner.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/controller/PIDControllerRunner.h" + +#include "frc/controller/PIDController.h" +#include "frc/smartdashboard/SendableBuilder.h" + +using namespace frc; + +PIDControllerRunner::PIDControllerRunner( + frc2::PIDController& controller, + std::function measurementSource, + std::function controllerOutput) + : m_controller(controller), + m_measurementSource(measurementSource), + m_controllerOutput(controllerOutput) { + m_notifier.StartPeriodic(m_controller.GetPeriod()); +} + +PIDControllerRunner::~PIDControllerRunner() { Disable(); } + +void PIDControllerRunner::Enable() { + std::lock_guard lock(m_thisMutex); + m_enabled = true; +} + +void PIDControllerRunner::Disable() { + // Ensures m_enabled modification and m_controllerOutput() call occur + // atomically + std::lock_guard outputLock(m_outputMutex); + { + std::lock_guard mainLock(m_thisMutex); + m_enabled = false; + } + + m_controllerOutput(0.0); +} + +bool PIDControllerRunner::IsEnabled() const { + std::lock_guard lock(m_thisMutex); + return m_enabled; +} + +void PIDControllerRunner::Run() { + // Ensures m_enabled check and m_controllerOutput() call occur atomically + std::lock_guard outputLock(m_outputMutex); + std::unique_lock mainLock(m_thisMutex); + if (m_enabled) { + // Don't block other PIDControllerRunner operations on output + mainLock.unlock(); + + m_controllerOutput(m_controller.Calculate(m_measurementSource())); + } +} + +void PIDControllerRunner::InitSendable(frc::SendableBuilder& builder) { + m_controller.InitSendable(builder); + builder.SetSafeState([this]() { Disable(); }); + builder.AddBooleanProperty("enabled", [this]() { return IsEnabled(); }, + [this](bool enabled) { + if (enabled) { + Enable(); + } else { + Disable(); + } + }); +} diff --git a/wpilibc/src/main/native/include/frc/Controller.h b/wpilibc/src/main/native/include/frc/Controller.h index b3277230ad..4124046dd0 100644 --- a/wpilibc/src/main/native/include/frc/Controller.h +++ b/wpilibc/src/main/native/include/frc/Controller.h @@ -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. */ @@ -7,6 +7,8 @@ #pragma once +#include + namespace frc { /** @@ -18,6 +20,7 @@ namespace frc { */ class Controller { public: + WPI_DEPRECATED("None of the 2020 FRC controllers use this.") Controller() = default; virtual ~Controller() = default; diff --git a/wpilibc/src/main/native/include/frc/PIDController.h b/wpilibc/src/main/native/include/frc/PIDController.h index 027032d151..e640f45c30 100644 --- a/wpilibc/src/main/native/include/frc/PIDController.h +++ b/wpilibc/src/main/native/include/frc/PIDController.h @@ -48,6 +48,7 @@ class PIDController : public PIDBase, public Controller { * particularly affects calculations of the integral and * differental terms. The default is 0.05 (50ms). */ + WPI_DEPRECATED("Use frc2::PIDController class instead.") PIDController(double p, double i, double d, PIDSource* source, PIDOutput* output, double period = 0.05); @@ -63,6 +64,7 @@ class PIDController : public PIDBase, public Controller { * particularly affects calculations of the integral and * differental terms. The default is 0.05 (50ms). */ + WPI_DEPRECATED("Use frc2::PIDController class instead.") PIDController(double p, double i, double d, double f, PIDSource* source, PIDOutput* output, double period = 0.05); @@ -78,6 +80,7 @@ class PIDController : public PIDBase, public Controller { * particularly affects calculations of the integral and * differental terms. The default is 0.05 (50ms). */ + WPI_DEPRECATED("Use frc2::PIDController class instead.") PIDController(double p, double i, double d, PIDSource& source, PIDOutput& output, double period = 0.05); @@ -93,6 +96,7 @@ class PIDController : public PIDBase, public Controller { * particularly affects calculations of the integral and * differental terms. The default is 0.05 (50ms). */ + WPI_DEPRECATED("Use frc2::PIDController class instead.") PIDController(double p, double i, double d, double f, PIDSource& source, PIDOutput& output, double period = 0.05); diff --git a/wpilibc/src/main/native/include/frc/controller/PIDController.h b/wpilibc/src/main/native/include/frc/controller/PIDController.h new file mode 100644 index 0000000000..57c992612c --- /dev/null +++ b/wpilibc/src/main/native/include/frc/controller/PIDController.h @@ -0,0 +1,307 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include + +#include "frc/smartdashboard/SendableBase.h" + +namespace frc2 { + +/** + * Implements a PID control loop. + */ +class PIDController : public frc::SendableBase { + public: + enum class Tolerance { kAbsolute, kPercent }; + + /** + * Allocates a PIDController with the given constants for Kp, Ki, and Kd. + * + * @param Kp The proportional coefficient. + * @param Ki The integral coefficient. + * @param Kd The derivative coefficient. + * @param period The period between controller updates in seconds. The + * default is 0.02 seconds. + */ + PIDController(double Kp, double Ki, double Kd, double period = 0.02); + + ~PIDController() override = default; + + PIDController(PIDController&& rhs); + PIDController& operator=(PIDController&& rhs); + + /** + * Sets the PID Controller gain parameters. + * + * Sets the proportional, integral, and differential coefficients. + * + * @param Kp Proportional coefficient + * @param Ki Integral coefficient + * @param Kd Differential coefficient + */ + void SetPID(double Kp, double Ki, double Kd); + + /** + * Sets the proportional coefficient of the PID controller gain. + * + * @param Kp proportional coefficient + */ + void SetP(double Kp); + + /** + * Sets the integral coefficient of the PID controller gain. + * + * @param Ki integral coefficient + */ + void SetI(double Ki); + + /** + * Sets the differential coefficient of the PID controller gain. + * + * @param Kd differential coefficient + */ + void SetD(double Kd); + + /** + * Gets the proportional coefficient. + * + * @return proportional coefficient + */ + double GetP() const; + + /** + * Gets the integral coefficient. + * + * @return integral coefficient + */ + double GetI() const; + + /** + * Gets the differential coefficient. + * + * @return differential coefficient + */ + double GetD() const; + + /** + * Gets the period of this controller. + * + * @return The period of the controller. + */ + double GetPeriod() const; + + /** + * Returns the current controller output. + * + * This is always centered around zero and constrained to the min and max + * outputs. + * + * @return The latest calculated output. + */ + double GetOutput() const; + + /** + * Sets the setpoint for the PIDController. + * + * @param setpoint The desired setpoint. + */ + void SetSetpoint(double setpoint); + + /** + * Returns the current setpoint of the PIDController. + * + * @return The current setpoint. + */ + double GetSetpoint() const; + + /** + * Returns true if the error is within tolerance of the setpoint. + * + * This will return false until at least one input value has been computed. + * + * @param tolerance The maximum allowable error. + * @param deltaTolerance The maximum allowable change in error. + * @param toleranceType The type of tolerances specified. + */ + bool AtSetpoint( + double tolerance, + double deltaTolerance = std::numeric_limits::infinity(), + Tolerance toleranceType = Tolerance::kAbsolute) const; + + /** + * Returns true if the error is within the tolerance of the error. + * + * Currently this just reports on target as the actual value passes through + * the setpoint. Ideally it should be based on being within the tolerance for + * some period of time. + * + * This will return false until at least one input value has been computed. + */ + bool AtSetpoint() const; + + /** + * Sets the PID controller to consider the input to be continuous. + * + * Rather then using the max and min input range as constraints, it considers + * them to be the same point and automatically calculates the shortest route + * to the setpoint. + * + * @param continuous true turns on continuous, false turns off continuous + */ + void SetContinuous(bool continuous = true); + + /** + * Sets the minimum and maximum values expected from the input. + * + * @param minimumInput The minimum value expected from the input. + * @param maximumInput The maximum value expected from the input. + */ + void SetInputRange(double minimumInput, double maximumInput); + + /** + * Sets the minimum and maximum values to write. + * + * @param minimumOutput the minimum value to write to the output + * @param maximumOutput the maximum value to write to the output + */ + void SetOutputRange(double minimumOutput, double maximumOutput); + + /** + * Sets the absolute error which is considered tolerable for use with + * AtSetpoint(). + * + * @param tolerance Error which is tolerable. + * @param deltaTolerance Change in error per second which is tolerable. + */ + void SetAbsoluteTolerance( + double tolerance, + double deltaTolerance = std::numeric_limits::infinity()); + + /** + * Sets the percentage error which is considered tolerable for use with + * AtSetpoint(). + * + * @param tolerance Percent error which is tolerable. + * @param deltaTolerance Change in percent error per second which is + * tolerable. + */ + void SetPercentTolerance( + double tolerance, + double deltaTolerance = std::numeric_limits::infinity()); + + /** + * Returns the difference between the setpoint and the measurement. + * + * @return The error. + */ + double GetError() const; + + /** + * Returns the change in error per second. + */ + double GetDeltaError() const; + + /** + * Returns the next output of the PID controller. + * + * @param measurement The current measurement of the process variable. + */ + double Calculate(double measurement); + + /** + * Returns the next output of the PID controller. + * + * @param measurement The current measurement of the process variable. + * @param setpoint The new setpoint of the controller. + */ + double Calculate(double measurement, double setpoint); + + /** + * Reset the previous error, the integral term, and disable the controller. + */ + void Reset(); + + void InitSendable(frc::SendableBuilder& builder) override; + + protected: + mutable wpi::mutex m_thisMutex; + + /** + * Wraps error around for continuous inputs. The original error is returned if + * continuous mode is disabled. This is an unsynchronized function. + * + * @param error The current error of the PID controller. + * @return Error for continuous inputs. + */ + double GetContinuousError(double error) const; + + private: + // Factor for "proportional" control + double m_Kp; + + // Factor for "integral" control + double m_Ki; + + // Factor for "derivative" control + double m_Kd; + + // The period (in seconds) of the control loop running this controller + double m_period; + + // |maximum output| + double m_maximumOutput = 1.0; + + // |minimum output| + double m_minimumOutput = -1.0; + + // Maximum input - limit setpoint to this + double m_maximumInput = 0; + + // Minimum input - limit setpoint to this + double m_minimumInput = 0; + + // input range - difference between maximum and minimum + double m_inputRange = 0; + + // Do the endpoints wrap around? eg. Absolute encoder + bool m_continuous = false; + + // The error at the time of the most recent call to calculate() + double m_currError = 0; + + // The error at the time of the second-most-recent call to calculate() (used + // to compute velocity) + double m_prevError = std::numeric_limits::infinity(); + + // The sum of the errors for use in the integral calc + double m_totalError = 0; + + Tolerance m_toleranceType = Tolerance::kAbsolute; + + // The percentage or absolute error that is considered at setpoint. + double m_tolerance = 0.05; + double m_deltaTolerance = std::numeric_limits::infinity(); + + double m_setpoint = 0; + double m_output = 0; + + /** + * Returns the next output of the PID controller. + * + * Unlike the public functions above, this function doesn't lock the mutex. + * + * @param measurement The current measurement of the process variable. + */ + double CalculateUnsafe(double measurement); +}; + +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc/controller/PIDControllerRunner.h b/wpilibc/src/main/native/include/frc/controller/PIDControllerRunner.h new file mode 100644 index 0000000000..aa47ca73a0 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/controller/PIDControllerRunner.h @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "frc/Notifier.h" +#include "frc/controller/PIDController.h" +#include "frc/smartdashboard/SendableBase.h" + +namespace frc { + +class PIDControllerRunner : SendableBase { + public: + /** + * Allocates a PIDControllerRunner. + * + * @param controller The controller on which to call Update(). + * @param controllerOutput The function which updates the plant using the + * controller output passed as the argument. + */ + PIDControllerRunner(frc2::PIDController& controller, + std::function measurementSource, + std::function controllerOutput); + + ~PIDControllerRunner(); + + PIDControllerRunner(PIDControllerRunner&&) = default; + PIDControllerRunner& operator=(PIDControllerRunner&&) = default; + + /** + * Begins running the controller. + */ + void Enable(); + + /** + * Stops running the controller. + * + * This sets the output to zero before stopping. + */ + void Disable(); + + /** + * Returns whether controller is running. + */ + bool IsEnabled() const; + + void InitSendable(SendableBuilder& builder) override; + + private: + Notifier m_notifier{&PIDControllerRunner::Run, this}; + frc2::PIDController& m_controller; + std::function m_measurementSource; + std::function m_controllerOutput; + bool m_enabled = false; + + mutable wpi::mutex m_thisMutex; + + // Ensures when Disable() is called, m_controllerOutput() won't run if + // Controller::Update() is already running at that time. + mutable wpi::mutex m_outputMutex; + + void Run(); +}; + +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp new file mode 100644 index 0000000000..73ccfb8293 --- /dev/null +++ b/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2014-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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/controller/PIDController.h" +#include "gtest/gtest.h" + +class PIDInputOutputTest : public testing::Test { + protected: + frc2::PIDController* controller; + + void SetUp() override { controller = new frc2::PIDController(0, 0, 0); } + + void TearDown() override { delete controller; } +}; + +TEST_F(PIDInputOutputTest, OutputRangeTest) { + controller->SetP(1); + controller->SetOutputRange(-50, 50); + + EXPECT_DOUBLE_EQ(-50, controller->Calculate(100, 0)); + EXPECT_DOUBLE_EQ(50, controller->Calculate(0, 100)); +} + +TEST_F(PIDInputOutputTest, InputRangeTest) { + controller->SetP(1); + controller->SetOutputRange(-1000, 1000); + controller->SetInputRange(-50, 50); + + EXPECT_DOUBLE_EQ(-100, controller->Calculate(100, 0)); + EXPECT_DOUBLE_EQ(50, controller->Calculate(0, 100)); +} + +TEST_F(PIDInputOutputTest, ContinuousInputTest) { + controller->SetP(1); + controller->SetInputRange(-180, 180); + controller->SetContinuous(true); + + EXPECT_TRUE(controller->Calculate(-179, 179) < 0); +} + +TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) { + controller->SetP(4); + + EXPECT_DOUBLE_EQ(-.1, controller->Calculate(.025, 0)); +} + +TEST_F(PIDInputOutputTest, IntegralGainOutputTest) { + controller->SetI(4); + + double out = 0; + + for (int i = 0; i < 5; i++) { + out = controller->Calculate(.025, 0); + } + + EXPECT_DOUBLE_EQ(-.5 * controller->GetPeriod(), out); +} + +TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) { + controller->SetD(4); + + controller->Calculate(0, 0); + + EXPECT_DOUBLE_EQ(-.01 / controller->GetPeriod(), + controller->Calculate(.0025, 0)); +} diff --git a/wpilibcIntegrationTests/src/main/native/cpp/PIDToleranceTest.cpp b/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp similarity index 53% rename from wpilibcIntegrationTests/src/main/native/cpp/PIDToleranceTest.cpp rename to wpilibc/src/test/native/cpp/PIDToleranceTest.cpp index ec3b85a94f..e978d9c02c 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/PIDToleranceTest.cpp +++ b/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp @@ -1,15 +1,13 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2014-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. */ /*----------------------------------------------------------------------------*/ -#include "TestBench.h" -#include "frc/PIDController.h" -#include "frc/PIDOutput.h" -#include "frc/PIDSource.h" #include "frc/Timer.h" +#include "frc/controller/PIDController.h" +#include "frc/controller/PIDControllerRunner.h" #include "gtest/gtest.h" using namespace frc; @@ -20,80 +18,86 @@ class PIDToleranceTest : public testing::Test { const double range = 200; const double tolerance = 10.0; - class FakeInput : public PIDSource { + class FakeInput { public: double val = 0; - void SetPIDSourceType(PIDSourceType pidSource) {} - - PIDSourceType GetPIDSourceType() { return PIDSourceType::kDisplacement; } - - double PIDGet() { return val; } + double Get() { return val; } }; - class FakeOutput : public PIDOutput { - void PIDWrite(double output) {} + class FakeOutput { + public: + void Set(double output) {} }; FakeInput inp; FakeOutput out; - PIDController* pid; + frc2::PIDController* pidController; + frc::PIDControllerRunner* pidRunner; void SetUp() override { - pid = new PIDController(0.5, 0.0, 0.0, &inp, &out); - pid->SetInputRange(-range / 2, range / 2); + pidController = new frc2::PIDController(0.5, 0.0, 0.0); + pidController->SetInputRange(-range / 2, range / 2); + pidRunner = + new frc::PIDControllerRunner(*pidController, [&] { return inp.Get(); }, + [&](double output) { out.Set(output); }); } - void TearDown() override { delete pid; } + void TearDown() override { + delete pidRunner; + delete pidController; + } void Reset() { inp.val = 0; } }; +TEST_F(PIDToleranceTest, Initial) { EXPECT_FALSE(pidController->AtSetpoint()); } + TEST_F(PIDToleranceTest, Absolute) { Reset(); - pid->SetAbsoluteTolerance(tolerance); - pid->SetSetpoint(setpoint); - pid->Enable(); + pidController->SetAbsoluteTolerance(tolerance); + pidController->SetSetpoint(setpoint); + pidRunner->Enable(); - EXPECT_FALSE(pid->OnTarget()) + EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " - << pid->GetError(); + << pidController->GetError(); inp.val = setpoint + tolerance / 2; Wait(1.0); - EXPECT_TRUE(pid->OnTarget()) + EXPECT_TRUE(pidController->AtSetpoint()) << "Error was not in tolerance when it should have been. Error was " - << pid->GetError(); + << pidController->GetError(); inp.val = setpoint + 10 * tolerance; Wait(1.0); - EXPECT_FALSE(pid->OnTarget()) + EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " - << pid->GetError(); + << pidController->GetError(); } TEST_F(PIDToleranceTest, Percent) { Reset(); - pid->SetPercentTolerance(tolerance); - pid->SetSetpoint(setpoint); - pid->Enable(); + pidController->SetPercentTolerance(tolerance); + pidController->SetSetpoint(setpoint); + pidRunner->Enable(); - EXPECT_FALSE(pid->OnTarget()) + EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " - << pid->GetError(); + << pidController->GetError(); inp.val = setpoint + (tolerance) / 200 * range; // half of percent tolerance away from setpoint Wait(1.0); - EXPECT_TRUE(pid->OnTarget()) + EXPECT_TRUE(pidController->AtSetpoint()) << "Error was not in tolerance when it should have been. Error was " - << pid->GetError(); + << pidController->GetError(); inp.val = setpoint + @@ -101,7 +105,7 @@ TEST_F(PIDToleranceTest, Percent) { Wait(1.0); - EXPECT_FALSE(pid->OnTarget()) + EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " - << pid->GetError(); + << pidController->GetError(); } diff --git a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp index a5b33de28e..d463a26d88 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp @@ -8,10 +8,11 @@ #include "TestBench.h" #include "frc/Encoder.h" #include "frc/Jaguar.h" -#include "frc/PIDController.h" #include "frc/Talon.h" #include "frc/Timer.h" #include "frc/Victor.h" +#include "frc/controller/PIDController.h" +#include "frc/controller/PIDControllerRunner.h" #include "frc/filters/LinearDigitalFilter.h" #include "gtest/gtest.h" @@ -139,22 +140,24 @@ TEST_P(MotorEncoderTest, ClampSpeed) { TEST_P(MotorEncoderTest, PositionPIDController) { Reset(); double goal = 1000; - m_encoder->SetPIDSourceType(PIDSourceType::kDisplacement); - PIDController pid(0.001, 0.0005, 0.0, m_encoder, m_speedController); - pid.SetAbsoluteTolerance(50.0); - pid.SetOutputRange(-0.2, 0.2); - pid.SetSetpoint(goal); + frc2::PIDController pidController(0.001, 0.01, 0.0); + pidController.SetAbsoluteTolerance(50.0); + pidController.SetOutputRange(-0.2, 0.2); + pidController.SetSetpoint(goal); - /* 10 seconds should be plenty time to get to the setpoint */ - pid.Enable(); + /* 10 seconds should be plenty time to get to the reference */ + frc::PIDControllerRunner pidRunner( + pidController, [&] { return m_encoder->GetDistance(); }, + [&](double output) { m_speedController->Set(output); }); + pidRunner.Enable(); Wait(10.0); - pid.Disable(); + pidRunner.Disable(); - RecordProperty("PIDError", pid.GetError()); + RecordProperty("PIDError", pidController.GetError()); - EXPECT_TRUE(pid.OnTarget()) + EXPECT_TRUE(pidController.AtSetpoint()) << "PID loop did not converge within 10 seconds. Goal was: " << goal - << " Error was: " << pid.GetError(); + << " Error was: " << pidController.GetError(); } /** @@ -164,20 +167,23 @@ TEST_P(MotorEncoderTest, VelocityPIDController) { Reset(); m_encoder->SetPIDSourceType(PIDSourceType::kRate); - PIDController pid(1e-5, 0.0, 3e-5, 8e-5, m_filter, m_speedController); - pid.SetAbsoluteTolerance(200.0); - pid.SetOutputRange(-0.3, 0.3); - pid.SetSetpoint(600); + frc2::PIDController pidController(1e-5, 0.0, 0.0006); + pidController.SetAbsoluteTolerance(200.0); + pidController.SetOutputRange(-0.3, 0.3); + pidController.SetSetpoint(600); - /* 10 seconds should be plenty time to get to the setpoint */ - pid.Enable(); + /* 10 seconds should be plenty time to get to the reference */ + frc::PIDControllerRunner pidRunner( + pidController, [&] { return m_filter->PIDGet(); }, + [&](double output) { m_speedController->Set(output + 8e-5); }); + pidRunner.Enable(); Wait(10.0); - pid.Disable(); - RecordProperty("PIDError", pid.GetError()); + pidRunner.Disable(); + RecordProperty("PIDError", pidController.GetError()); - EXPECT_TRUE(pid.OnTarget()) + EXPECT_TRUE(pidController.AtSetpoint()) << "PID loop did not converge within 10 seconds. Goal was: " << 600 - << " Error was: " << pid.GetError(); + << " Error was: " << pidController.GetError(); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java index 32fc7c3cfb..31d50d82f0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.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. */ @@ -11,7 +11,10 @@ package edu.wpi.first.wpilibj; * An interface for controllers. Controllers run control loops, the most command are PID controllers * and there variants, but this includes anything that is controlling an actuator in a separate * thread. + * + * @deprecated None of the 2020 FRC controllers use this. */ +@Deprecated(since = "2020", forRemoval = true) public interface Controller { /** * Allows the control loop to run. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java index da4118f8c7..b87e406e1a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.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. */ @@ -18,7 +18,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; *

This feedback controller runs in discrete time, so time deltas are not used in the integral * and derivative calculations. Therefore, the sample rate affects the controller's behavior for a * given set of PID constants. + * + * @deprecated Use {@link edu.wpi.first.wpilibj.controller.PIDController} instead. */ +@Deprecated(since = "2020", forRemoval = true) public class PIDController extends PIDBase implements Controller { Notifier m_controlLoop = new Notifier(this::calculate); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java index 0ef9403bda..e9af2ff7e5 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.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. */ @@ -9,8 +9,11 @@ package edu.wpi.first.wpilibj; /** * This interface allows PIDController to write it's results to its output. + * + * @deprecated Use DoubleConsumer and new PIDController class. */ @FunctionalInterface +@Deprecated(since = "2020", forRemoval = true) public interface PIDOutput { /** * Set the output to the value calculated by PIDController. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java index 841a232b8f..3c4f1f5ce1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.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. */ @@ -9,7 +9,10 @@ package edu.wpi.first.wpilibj; /** * This interface allows for PIDController to automatically read from this object. + * + * @deprecated Use DoubleSupplier and new PIDController class. */ +@Deprecated(since = "2020", forRemoval = true) public interface PIDSource { /** * Set which parameter of the device you are using as a process control variable. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java index 31aa79abd2..7508be54b2 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2015-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. */ @@ -10,6 +10,7 @@ package edu.wpi.first.wpilibj; /** * A description for the type of output value to provide to a PIDController. */ +@Deprecated(since = "2020", forRemoval = true) public enum PIDSourceType { kDisplacement, kRate 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 new file mode 100644 index 0000000000..4e8507b8ec --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java @@ -0,0 +1,570 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-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 java.util.concurrent.locks.ReentrantLock; + +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.SendableBase; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; + +/** + * Implements a PID control loop. + */ +@SuppressWarnings("PMD.TooManyFields") +public class PIDController extends SendableBase { + protected final ReentrantLock m_thisMutex = new ReentrantLock(); + + private static int instances; + + // Factor for "proportional" control + @SuppressWarnings("MemberName") + private double m_Kp; + + // Factor for "integral" control + @SuppressWarnings("MemberName") + private double m_Ki; + + // Factor for "derivative" control + @SuppressWarnings("MemberName") + private double m_Kd; + + // The period (in seconds) of the loop that calls the controller + private final double m_period; + + // |maximum output| + private double m_maximumOutput = 1.0; + + // |minimum output| + private double m_minimumOutput = -1.0; + + // Maximum input - limit setpoint to this + private double m_maximumInput; + + // Minimum input - limit setpoint to this + private double m_minimumInput; + + // input range - difference between maximum and minimum + private double m_inputRange; + + // Do the endpoints wrap around? eg. Absolute encoder + private boolean m_continuous; + + // The error at the time of the most recent call to calculate() + private double m_currError; + + // The error at the time of the second-most-recent call to calculate() (used to compute velocity) + private double m_prevError = Double.POSITIVE_INFINITY; + + // The sum of the errors for use in the integral calc + private double m_totalError; + + enum Tolerance { + kAbsolute, kPercent; + } + + private Tolerance m_toleranceType = Tolerance.kAbsolute; + + // The percentage or absolute error that is considered at setpoint. + private double m_tolerance = 0.05; + private double m_deltaTolerance = Double.POSITIVE_INFINITY; + + private double m_setpoint; + private double m_output; + + /** + * Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of + * 0.02 seconds. + * + * @param Kp The proportional coefficient. + * @param Ki The integral coefficient. + * @param Kd The derivative coefficient. + */ + @SuppressWarnings("ParameterName") + public PIDController(double Kp, double Ki, double Kd) { + this(Kp, Ki, Kd, 0.02); + } + + /** + * Allocates a PIDController with the given constants for Kp, Ki, and Kd. + * + * @param Kp The proportional coefficient. + * @param Ki The integral coefficient. + * @param Kd The derivative coefficient. + * @param period The period between controller updates in seconds. + */ + @SuppressWarnings("ParameterName") + public PIDController(double Kp, double Ki, double Kd, double period) { + m_Kp = Kp; + m_Ki = Ki; + m_Kd = Kd; + + m_period = period; + + instances++; + HAL.report(tResourceType.kResourceType_PIDController, instances); + } + + /** + * Sets the PID Controller gain parameters. + * + *

Set the proportional, integral, and differential coefficients. + * + * @param Kp The proportional coefficient. + * @param Ki The integral coefficient. + * @param Kd The derivative coefficient. + */ + @SuppressWarnings("ParameterName") + public void setPID(double Kp, double Ki, double Kd) { + m_thisMutex.lock(); + try { + m_Kp = Kp; + m_Ki = Ki; + m_Kd = Kd; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Sets the Proportional coefficient of the PID controller gain. + * + * @param Kp proportional coefficient + */ + @SuppressWarnings("ParameterName") + public void setP(double Kp) { + m_thisMutex.lock(); + try { + m_Kp = Kp; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Sets the Integral coefficient of the PID controller gain. + * + * @param Ki integral coefficient + */ + @SuppressWarnings("ParameterName") + public void setI(double Ki) { + m_thisMutex.lock(); + try { + m_Ki = Ki; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Sets the Differential coefficient of the PID controller gain. + * + * @param Kd differential coefficient + */ + @SuppressWarnings("ParameterName") + public void setD(double Kd) { + m_thisMutex.lock(); + try { + m_Kd = Kd; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Get the Proportional coefficient. + * + * @return proportional coefficient + */ + public double getP() { + m_thisMutex.lock(); + try { + return m_Kp; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Get the Integral coefficient. + * + * @return integral coefficient + */ + public double getI() { + m_thisMutex.lock(); + try { + return m_Ki; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Get the Differential coefficient. + * + * @return differential coefficient + */ + public double getD() { + m_thisMutex.lock(); + try { + return m_Kd; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Returns the period of this controller. + * + * @return the period of the controller. + */ + public double getPeriod() { + return m_period; + } + + /** + * Returns the current controller output. + * + *

This is always centered around zero and constrained to the min and max outputs. + * + * @return The latest calculated output. + */ + public double getOutput() { + m_thisMutex.lock(); + try { + return m_output; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Sets the setpoint for the PIDController. + * + * @param setpoint The desired setpoint. + */ + public void setSetpoint(double setpoint) { + m_thisMutex.lock(); + try { + if (m_maximumInput > m_minimumInput) { + m_setpoint = clamp(setpoint, m_minimumInput, m_maximumInput); + } else { + m_setpoint = setpoint; + } + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Returns the current setpoint of the PIDController. + * + * @return The current setpoint. + */ + public double getSetpoint() { + m_thisMutex.lock(); + try { + return m_setpoint; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Returns true if the error is within the percentage of the total input range, + * determined by SetTolerance. This asssumes that the maximum and minimum + * input were set using SetInput. + * + *

This will return false until at least one input value has been computed. + * + * @return Whether the error is within the acceptable bounds. + */ + public boolean atSetpoint() { + return atSetpoint(m_tolerance, m_deltaTolerance, m_toleranceType); + } + + /** + * Returns true if the error and change in error are below the specified tolerances. + * + * @param tolerance The maximum allowable error. + * @param deltaTolerance The maximum allowable change in error from the previous iteration. + * @param toleranceType Whether the given tolerance values are absolute, or percentages of the + * total input range. + * @return Whether the error is within the acceptable bounds. + */ + public boolean atSetpoint(double tolerance, double deltaTolerance, Tolerance toleranceType) { + double error = getError(); + + m_thisMutex.lock(); + try { + double deltaError = (error - m_prevError) / getPeriod(); + if (toleranceType == Tolerance.kPercent) { + return Math.abs(error) < tolerance / 100 * m_inputRange + && Math.abs(deltaError) < deltaTolerance / 100 * m_inputRange; + } else { + return Math.abs(error) < tolerance + && Math.abs(deltaError) < deltaTolerance; + } + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Sets the PID controller to consider the input to be continuous. + * + *

Rather then using the max and min input range as constraints, it considers them to be the + * same point and automatically calculates the shortest route to the setpoint. + */ + public void setContinuous() { + setContinuous(true); + } + + /** + * Sets the PID controller to consider the input to be continuous, + * + *

Rather then using the max and min input range as constraints, it considers them to be the + * same point and automatically calculates the shortest route to the setpoint. + * + * @param continuous true turns on continuous, false turns off continuous + */ + public void setContinuous(boolean continuous) { + m_thisMutex.lock(); + try { + m_continuous = continuous; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Sets the minimum and maximum values expected from the input. + * + * @param minimumInput The minimum value expected from the input. + * @param maximumInput The maximum value expected from the input. + */ + public void setInputRange(double minimumInput, double maximumInput) { + m_thisMutex.lock(); + try { + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; + m_inputRange = maximumInput - minimumInput; + + // Clamp setpoint to new input + if (m_maximumInput > m_minimumInput) { + m_setpoint = clamp(m_setpoint, m_minimumInput, m_maximumInput); + } + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Sets the minimum and maximum values to write. + * + * @param minimumOutput the minimum value to write to the output + * @param maximumOutput the maximum value to write to the output + */ + public void setOutputRange(double minimumOutput, double maximumOutput) { + m_thisMutex.lock(); + try { + m_minimumOutput = minimumOutput; + m_maximumOutput = maximumOutput; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Sets the absolute error which is considered tolerable for use with atSetpoint(). + * + * @param tolerance Absolute error which is tolerable. + */ + public void setAbsoluteTolerance(double tolerance) { + setAbsoluteTolerance(tolerance, Double.POSITIVE_INFINITY); + } + + /** + * Sets the absolute error which is considered tolerable for use with atSetpoint(). + * + * @param tolerance Absolute error which is tolerable. + * @param deltaTolerance Change in absolute error per second which is tolerable. + */ + public void setAbsoluteTolerance(double tolerance, double deltaTolerance) { + m_thisMutex.lock(); + try { + m_toleranceType = Tolerance.kAbsolute; + m_tolerance = tolerance; + m_deltaTolerance = deltaTolerance; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Sets the percent error which is considered tolerable for use with atSetpoint(). + * + * @param tolerance Percent error which is tolerable. + */ + public void setPercentTolerance(double tolerance) { + setPercentTolerance(tolerance, Double.POSITIVE_INFINITY); + } + + /** + * Sets the percent error which is considered tolerable for use with atSetpoint(). + * + * @param tolerance Percent error which is tolerable. + * @param deltaTolerance Change in percent error per second which is tolerable. + */ + public void setPercentTolerance(double tolerance, double deltaTolerance) { + m_thisMutex.lock(); + try { + m_toleranceType = Tolerance.kPercent; + m_tolerance = tolerance; + m_deltaTolerance = deltaTolerance; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Returns the difference between the setpoint and the measurement. + * + * @return The error. + */ + public double getError() { + m_thisMutex.lock(); + try { + return getContinuousError(m_currError); + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Returns the change in error per second. + */ + public double getDeltaError() { + double error = getError(); + + m_thisMutex.lock(); + try { + return (error - m_prevError) / getPeriod(); + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Returns the next output of the PID controller. + * + * @param measurement The current measurement of the process variable. + */ + public double calculate(double measurement) { + m_thisMutex.lock(); + try { + return calculateUnsafe(measurement); + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Returns the next output of the PID controller. + * + * @param measurement The current measurement of the process variable. + * @param setpoint The new setpoint of the controller. + */ + public double calculate(double measurement, double setpoint) { + m_thisMutex.lock(); + try { + // Set setpoint to provided value + if (m_maximumInput > m_minimumInput) { + m_setpoint = clamp(setpoint, m_minimumInput, m_maximumInput); + } else { + m_setpoint = setpoint; + } + + return calculateUnsafe(measurement); + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Resets the previous error and the integral term. Also disables the controller. + */ + public void reset() { + m_thisMutex.lock(); + try { + m_prevError = 0; + m_totalError = 0; + m_output = 0; + } finally { + m_thisMutex.unlock(); + } + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("PIDController"); + builder.addDoubleProperty("p", this::getP, this::setP); + builder.addDoubleProperty("i", this::getI, this::setI); + builder.addDoubleProperty("d", this::getD, this::setD); + builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint); + } + + /** + * Wraps error around for continuous inputs. The original error is returned if continuous mode is + * disabled. This is an unsynchronized function. + * + * @param error The current error of the PID controller. + * @return Error for continuous inputs. + */ + protected double getContinuousError(double error) { + if (m_continuous && m_inputRange > 0) { + error %= m_inputRange; + if (Math.abs(error) > m_inputRange / 2) { + if (error > 0) { + return error - m_inputRange; + } else { + return error + m_inputRange; + } + } + } + + return error; + } + + /** + * Returns the next output of the PID controller. + * + *

Unlike the public functions above, this function doesn't lock the mutex. + * + * @param measurement The current measurement of the process variable. + */ + private double calculateUnsafe(double measurement) { + m_prevError = m_currError; + m_currError = getContinuousError(m_setpoint - measurement); + + if (m_Ki != 0) { + m_totalError = clamp(m_totalError + m_currError * getPeriod(), m_minimumOutput / m_Ki, + m_maximumOutput / m_Ki); + } + + m_output = clamp(m_Kp * m_currError + m_Ki * m_totalError + + m_Kd * (m_currError - m_prevError) / getPeriod(), m_minimumOutput, m_maximumOutput); + + return m_output; + } + + private static double clamp(double value, double low, double high) { + return Math.max(low, Math.min(value, high)); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDControllerRunner.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDControllerRunner.java new file mode 100644 index 0000000000..7d6edb866d --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDControllerRunner.java @@ -0,0 +1,129 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-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 java.util.concurrent.locks.ReentrantLock; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.SendableBase; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; + +public class PIDControllerRunner extends SendableBase { + private final Notifier m_notifier = new Notifier(this::run); + private final PIDController m_controller; + private final DoubleConsumer m_controllerOutput; + private final DoubleSupplier m_measurementSource; + private boolean m_enabled; + + private final ReentrantLock m_thisMutex = new ReentrantLock(); + + // Ensures when disable() is called, m_controllerOutput() won't run if + // Controller.update() is already running at that time. + private final ReentrantLock m_outputMutex = new ReentrantLock(); + + /** + * Allocates a PIDControllerRunner. + * + * @param controller The controller on which to call update(). + * @param measurementSource The function that supplies the current process variable measurement. + * @param controllerOutput The function which updates the plant using the controller output + * passed as the argument. + */ + public PIDControllerRunner(PIDController controller, DoubleSupplier measurementSource, + DoubleConsumer controllerOutput) { + m_controller = controller; + m_controllerOutput = controllerOutput; + m_measurementSource = measurementSource; + m_notifier.startPeriodic(m_controller.getPeriod()); + } + + /** + * Begins running the controller. + */ + public void enable() { + m_thisMutex.lock(); + try { + m_enabled = true; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Stops running the controller. + * + *

This sets the output to zero before stopping. + */ + public void disable() { + // Ensures m_enabled modification and m_controllerOutput() call occur + // atomically + m_outputMutex.lock(); + try { + m_thisMutex.lock(); + try { + m_enabled = false; + } finally { + m_thisMutex.unlock(); + } + + m_controllerOutput.accept(0.0); + } finally { + m_outputMutex.unlock(); + } + } + + /** + * Returns whether controller is running. + */ + public boolean isEnabled() { + m_thisMutex.lock(); + try { + return m_enabled; + } finally { + m_thisMutex.unlock(); + } + } + + + private void run() { + // Ensures m_enabled check and m_controllerOutput() call occur atomically + m_outputMutex.lock(); + try { + m_thisMutex.lock(); + try { + if (m_enabled) { + // Don't block other PIDControllerRunner operations on output + m_thisMutex.unlock(); + + m_controllerOutput.accept(m_controller.calculate(m_measurementSource.getAsDouble())); + } + } finally { + if (m_thisMutex.isHeldByCurrentThread()) { + m_thisMutex.unlock(); + } + } + } finally { + m_outputMutex.unlock(); + } + } + + @Override + public void initSendable(SendableBuilder builder) { + m_controller.initSendable(builder); + builder.setSafeState(this::disable); + builder.addBooleanProperty("enabled", this::isEnabled, enabled -> { + if (enabled) { + enable(); + } else { + disable(); + } + }); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java new file mode 100644 index 0000000000..6c73553aad --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java @@ -0,0 +1,87 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-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; + +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.controller.PIDController; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class PIDInputOutputTest { + private PIDController m_controller; + + @BeforeEach + void setUp() { + m_controller = new PIDController(0, 0, 0); + } + + @Test + void outputRangeTest() { + m_controller.setP(1); + m_controller.setOutputRange(-50, 50); + + assertAll( + () -> assertEquals(-50, m_controller.calculate(100, 0), 1e-5), + () -> assertEquals(50, m_controller.calculate(0, 100), 1e-5) + ); + } + + @Test + void inputRangeTest() { + m_controller.setP(1); + m_controller.setOutputRange(-1000, 1000); + m_controller.setInputRange(-50, 50); + + assertAll( + () -> assertEquals(-100, m_controller.calculate(100, 0), 1e-5), + () -> assertEquals(50, m_controller.calculate(0, 100), 1e-5) + ); + } + + @Test + void continuousInputTest() { + m_controller.setP(1); + m_controller.setInputRange(-180, 180); + m_controller.setContinuous(true); + + assertTrue(m_controller.calculate(-179, 179) < 0.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(.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); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java index cbf3f99531..838aca4abc 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2016-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. */ @@ -11,93 +11,92 @@ import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.PIDControllerRunner; + import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; class PIDToleranceTest { - private PIDController m_pid; - private static final double m_setPoint = 50.0; + private PIDController m_pidController; + private PIDControllerRunner m_pidRunner; + private static final double m_setpoint = 50.0; private static final double m_tolerance = 10.0; private static final double m_range = 200; - private class FakeInput implements PIDSource { + private static class FakeInput { public double m_val; FakeInput() { m_val = 0; } - @Override - public PIDSourceType getPIDSourceType() { - return PIDSourceType.kDisplacement; - } - - @Override - public double pidGet() { + public double getMeasurement() { return m_val; } - - @Override - public void setPIDSourceType(PIDSourceType arg0) { - } } private FakeInput m_inp; - private final PIDOutput m_out = new PIDOutput() { - @Override - public void pidWrite(double out) { - } - - }; - @BeforeEach void setUp() { m_inp = new FakeInput(); - m_pid = new PIDController(0.05, 0.0, 0.0, m_inp, m_out); - m_pid.setInputRange(-m_range / 2, m_range / 2); + m_pidController = new PIDController(0.05, 0.0, 0.0); + m_pidRunner = new PIDControllerRunner(m_pidController, m_inp::getMeasurement, x -> { }); + m_pidController.setInputRange(-m_range / 2, m_range / 2); } @AfterEach void tearDown() { - m_pid.close(); - m_pid = null; + m_pidController.close(); + m_pidController = null; + } + + @Test + void initialToleranceTest() { + assertFalse(m_pidController.atSetpoint()); } @Test void absoluteToleranceTest() { - m_pid.setAbsoluteTolerance(m_tolerance); - m_pid.setSetpoint(m_setPoint); - m_pid.enable(); + m_pidController.setAbsoluteTolerance(m_tolerance); + m_pidController.setSetpoint(m_setpoint); + m_pidRunner.enable(); Timer.delay(1); - assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was " - + m_pid.getError()); - m_inp.m_val = m_setPoint + m_tolerance / 2; + assertFalse(m_pidController.atSetpoint(), + "Error was in tolerance when it should not have been. Error was " + + m_pidController.getError()); + m_inp.m_val = m_setpoint + m_tolerance / 2; Timer.delay(1.0); - assertTrue(m_pid.onTarget(), "Error was not in tolerance when it should have been. Error was " - + m_pid.getError()); - m_inp.m_val = m_setPoint + 10 * m_tolerance; + assertTrue(m_pidController.atSetpoint(), + "Error was not in tolerance when it should have been. Error was " + + m_pidController.getError()); + m_inp.m_val = m_setpoint + 10 * m_tolerance; Timer.delay(1.0); - assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was " - + m_pid.getError()); + assertFalse(m_pidController.atSetpoint(), + "Error was in tolerance when it should not have been. Error was " + + m_pidController.getError()); } @Test void percentToleranceTest() { - m_pid.setPercentTolerance(m_tolerance); - m_pid.setSetpoint(m_setPoint); - m_pid.enable(); - assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was " - + m_pid.getError()); + m_pidController.setPercentTolerance(m_tolerance); + m_pidController.setSetpoint(m_setpoint); + m_pidRunner.enable(); + assertFalse(m_pidController.atSetpoint(), + "Error was in tolerance when it should not have been. Error was " + + m_pidController.getError()); //half of percent tolerance away from setPoint - m_inp.m_val = m_setPoint + m_tolerance / 200 * m_range; + m_inp.m_val = m_setpoint + m_tolerance / 200 * m_range; Timer.delay(1.0); - assertTrue(m_pid.onTarget(), "Error was not in tolerance when it should have been. Error was " - + m_pid.getError()); + assertTrue(m_pidController.atSetpoint(), + "Error was not in tolerance when it should have been. Error was " + + m_pidController.getError()); //double percent tolerance away from setPoint - m_inp.m_val = m_setPoint + m_tolerance / 50 * m_range; + m_inp.m_val = m_setpoint + m_tolerance / 50 * m_range; Timer.delay(1.0); - assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was " - + m_pid.getError()); + assertFalse(m_pidController.atSetpoint(), + "Error was in tolerance when it should not have been. Error was " + + m_pidController.getError()); } } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java index 59400c7c15..4f63d94178 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java +++ b/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(); } /** diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java index ad05703306..f5805f6b14 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java +++ b/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(); } }