diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp index 4d25298396..2e8d07c2b4 100644 --- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp +++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp @@ -24,94 +24,23 @@ PIDController::PIDController(double Kp, double Ki, double Kd, double period) 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)) {} +void PIDController::SetP(double Kp) { m_Kp = Kp; } -PIDController& PIDController::operator=(PIDController&& rhs) { - std::scoped_lock lock(m_thisMutex, rhs.m_thisMutex); +void PIDController::SetI(double Ki) { m_Ki = Ki; } - SendableBase::operator=(std::move(rhs)); +void PIDController::SetD(double Kd) { m_Kd = Kd; } - 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); +double PIDController::GetP() const { return m_Kp; } - return *this; -} +double PIDController::GetI() const { return m_Ki; } -void PIDController::SetP(double Kp) { - std::scoped_lock lock(m_thisMutex); - m_Kp = Kp; -} - -void PIDController::SetI(double Ki) { - std::scoped_lock lock(m_thisMutex); - m_Ki = Ki; -} - -void PIDController::SetD(double Kd) { - std::scoped_lock lock(m_thisMutex); - m_Kd = Kd; -} - -double PIDController::GetP() const { - std::scoped_lock lock(m_thisMutex); - return m_Kp; -} - -double PIDController::GetI() const { - std::scoped_lock lock(m_thisMutex); - return m_Ki; -} - -double PIDController::GetD() const { - std::scoped_lock lock(m_thisMutex); - return m_Kd; -} +double PIDController::GetD() const { return m_Kd; } double PIDController::GetPeriod() const { return m_period; } -double PIDController::GetOutput() const { - std::scoped_lock lock(m_thisMutex); - return m_output; -} +double PIDController::GetOutput() const { return m_output; } void PIDController::SetSetpoint(double setpoint) { - std::scoped_lock lock(m_thisMutex); - if (m_maximumInput > m_minimumInput) { m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput); } else { @@ -119,16 +48,12 @@ void PIDController::SetSetpoint(double setpoint) { } } -double PIDController::GetSetpoint() const { - std::scoped_lock lock(m_thisMutex); - return m_setpoint; -} +double PIDController::GetSetpoint() const { return m_setpoint; } bool PIDController::AtSetpoint(double tolerance, double deltaTolerance, Tolerance toleranceType) const { double deltaError = GetDeltaError(); - std::scoped_lock lock(m_thisMutex); if (toleranceType == Tolerance::kPercent) { return std::abs(m_currError) < tolerance / 100 * m_inputRange && std::abs(deltaError) < deltaTolerance / 100 * m_inputRange; @@ -143,12 +68,10 @@ bool PIDController::AtSetpoint() const { } void PIDController::SetContinuous(bool continuous) { - std::scoped_lock lock(m_thisMutex); m_continuous = continuous; } void PIDController::SetInputRange(double minimumInput, double maximumInput) { - std::scoped_lock lock(m_thisMutex); m_minimumInput = minimumInput; m_maximumInput = maximumInput; m_inputRange = maximumInput - minimumInput; @@ -160,14 +83,12 @@ void PIDController::SetInputRange(double minimumInput, double maximumInput) { } void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) { - std::scoped_lock lock(m_thisMutex); m_minimumOutput = minimumOutput; m_maximumOutput = maximumOutput; } void PIDController::SetAbsoluteTolerance(double tolerance, double deltaTolerance) { - std::scoped_lock lock(m_thisMutex); m_toleranceType = Tolerance::kAbsolute; m_tolerance = tolerance; m_deltaTolerance = deltaTolerance; @@ -175,14 +96,12 @@ void PIDController::SetAbsoluteTolerance(double tolerance, void PIDController::SetPercentTolerance(double tolerance, double deltaTolerance) { - std::scoped_lock lock(m_thisMutex); m_toleranceType = Tolerance::kPercent; m_tolerance = tolerance; m_deltaTolerance = deltaTolerance; } double PIDController::GetError() const { - std::scoped_lock lock(m_thisMutex); return GetContinuousError(m_currError); } @@ -192,30 +111,16 @@ double PIDController::GetError() const { * @return The change in error per second. */ double PIDController::GetDeltaError() const { - std::scoped_lock lock(m_thisMutex); return (m_currError - m_prevError) / GetPeriod(); } -double PIDController::Calculate(double measurement) { - std::scoped_lock lock(m_thisMutex); - return CalculateUnsafe(measurement); -} - double PIDController::Calculate(double measurement, double setpoint) { - std::scoped_lock 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); + SetSetpoint(setpoint); + return Calculate(measurement); } void PIDController::Reset() { - std::scoped_lock lock(m_thisMutex); m_prevError = 0; m_totalError = 0; m_output = 0; @@ -248,7 +153,7 @@ double PIDController::GetContinuousError(double error) const { return error; } -double PIDController::CalculateUnsafe(double measurement) { +double PIDController::Calculate(double measurement) { m_prevError = m_currError; m_currError = GetContinuousError(m_setpoint - measurement); diff --git a/wpilibc/src/main/native/cpp/controller/PIDControllerRunner.cpp b/wpilibc/src/main/native/cpp/controller/PIDControllerRunner.cpp deleted file mode 100644 index 06d621fb08..0000000000 --- a/wpilibc/src/main/native/cpp/controller/PIDControllerRunner.cpp +++ /dev/null @@ -1,72 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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::scoped_lock lock(m_thisMutex); - m_enabled = true; -} - -void PIDControllerRunner::Disable() { - // Ensures m_enabled modification and m_controllerOutput() call occur - // atomically - std::scoped_lock outputLock(m_outputMutex); - { - std::scoped_lock mainLock(m_thisMutex); - m_enabled = false; - } - - m_controllerOutput(0.0); -} - -bool PIDControllerRunner::IsEnabled() const { - std::scoped_lock lock(m_thisMutex); - return m_enabled; -} - -void PIDControllerRunner::Run() { - // Ensures m_enabled check and m_controllerOutput() call occur atomically - std::scoped_lock 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/PIDController.h b/wpilibc/src/main/native/include/frc/controller/PIDController.h index 57c992612c..7bffbc01f7 100644 --- a/wpilibc/src/main/native/include/frc/controller/PIDController.h +++ b/wpilibc/src/main/native/include/frc/controller/PIDController.h @@ -10,8 +10,6 @@ #include #include -#include - #include "frc/smartdashboard/SendableBase.h" namespace frc2 { @@ -36,8 +34,10 @@ class PIDController : public frc::SendableBase { ~PIDController() override = default; - PIDController(PIDController&& rhs); - PIDController& operator=(PIDController&& rhs); + PIDController(const PIDController&) = default; + PIDController& operator=(const PIDController&) = default; + PIDController(PIDController&&) = default; + PIDController& operator=(PIDController&&) = default; /** * Sets the PID Controller gain parameters. @@ -233,11 +233,9 @@ class PIDController : public frc::SendableBase { 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. + * continuous mode is disabled. * * @param error The current error of the PID controller. * @return Error for continuous inputs. @@ -293,15 +291,6 @@ class PIDController : public frc::SendableBase { 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 deleted file mode 100644 index bcfacb9aab..0000000000 --- a/wpilibc/src/main/native/include/frc/controller/PIDControllerRunner.h +++ /dev/null @@ -1,78 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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: - 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; - - // This is declared after all other member variables so that during - // PIDControllerRunner destruction, the Notifier is stopped before any member - // variables its callable uses are destructed. This avoids use-after-free - // bugs like crashes when locking is attempted on deallocated mutexes. - Notifier m_notifier{&PIDControllerRunner::Run, this}; - - void Run(); -}; - -} // namespace frc diff --git a/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp b/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp index e978d9c02c..a1be0b9aed 100644 --- a/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp +++ b/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp @@ -7,7 +7,6 @@ #include "frc/Timer.h" #include "frc/controller/PIDController.h" -#include "frc/controller/PIDControllerRunner.h" #include "gtest/gtest.h" using namespace frc; @@ -33,20 +32,13 @@ class PIDToleranceTest : public testing::Test { FakeInput inp; FakeOutput out; frc2::PIDController* pidController; - frc::PIDControllerRunner* pidRunner; void SetUp() override { 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 pidRunner; - delete pidController; - } + void TearDown() override { delete pidController; } void Reset() { inp.val = 0; } }; @@ -58,21 +50,24 @@ TEST_F(PIDToleranceTest, Absolute) { pidController->SetAbsoluteTolerance(tolerance); pidController->SetSetpoint(setpoint); - pidRunner->Enable(); EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " << pidController->GetError(); inp.val = setpoint + tolerance / 2; - Wait(1.0); + for (int i = 0; i <= 50; i++) { + pidController->Calculate(inp.Get()); + } EXPECT_TRUE(pidController->AtSetpoint()) << "Error was not in tolerance when it should have been. Error was " << pidController->GetError(); inp.val = setpoint + 10 * tolerance; - Wait(1.0); + for (int i = 0; i <= 50; i++) { + pidController->Calculate(inp.Get()); + } EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " @@ -84,7 +79,6 @@ TEST_F(PIDToleranceTest, Percent) { pidController->SetPercentTolerance(tolerance); pidController->SetSetpoint(setpoint); - pidRunner->Enable(); EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " @@ -93,7 +87,9 @@ TEST_F(PIDToleranceTest, Percent) { inp.val = setpoint + (tolerance) / 200 * range; // half of percent tolerance away from setpoint - Wait(1.0); + for (int i = 0; i <= 50; i++) { + pidController->Calculate(inp.Get()); + } EXPECT_TRUE(pidController->AtSetpoint()) << "Error was not in tolerance when it should have been. Error was " @@ -103,7 +99,9 @@ TEST_F(PIDToleranceTest, Percent) { setpoint + (tolerance) / 50 * range; // double percent tolerance away from setPoint - Wait(1.0); + for (int i = 0; i <= 50; i++) { + pidController->Calculate(inp.Get()); + } EXPECT_FALSE(pidController->AtSetpoint()) << "Error was in tolerance when it should not have been. Error was " diff --git a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp index 626743f602..36c7027545 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp @@ -9,11 +9,11 @@ #include "frc/Encoder.h" #include "frc/Jaguar.h" #include "frc/LinearFilter.h" +#include "frc/Notifier.h" #include "frc/Talon.h" #include "frc/Timer.h" #include "frc/Victor.h" #include "frc/controller/PIDController.h" -#include "frc/controller/PIDControllerRunner.h" #include "gtest/gtest.h" using namespace frc; @@ -145,12 +145,12 @@ TEST_P(MotorEncoderTest, PositionPIDController) { pidController.SetSetpoint(goal); /* 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(); + frc::Notifier pidRunner{[this, &pidController] { + m_speedController->Set(pidController.Calculate(m_encoder->GetDistance())); + }}; + pidRunner.StartPeriodic(pidController.GetPeriod()); Wait(10.0); - pidRunner.Disable(); + pidRunner.Stop(); RecordProperty("PIDError", pidController.GetError()); @@ -171,12 +171,14 @@ TEST_P(MotorEncoderTest, VelocityPIDController) { pidController.SetSetpoint(600); /* 10 seconds should be plenty time to get to the reference */ - frc::PIDControllerRunner pidRunner( - pidController, [&] { return m_filter->Calculate(m_encoder->GetRate()); }, - [&](double output) { m_speedController->Set(output + 8e-5); }); - pidRunner.Enable(); + frc::Notifier pidRunner{[this, &pidController] { + m_speedController->Set( + pidController.Calculate(m_filter->Calculate(m_encoder->GetRate())) + + 8e-5); + }}; + pidRunner.StartPeriodic(pidController.GetPeriod()); Wait(10.0); - pidRunner.Disable(); + pidRunner.Stop(); RecordProperty("PIDError", pidController.GetError()); EXPECT_TRUE(pidController.AtSetpoint()) 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 c35f9d8e36..2613d930bd 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 @@ -7,8 +7,6 @@ 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; @@ -19,8 +17,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; */ @SuppressWarnings("PMD.TooManyFields") public class PIDController extends SendableBase { - protected final ReentrantLock m_thisMutex = new ReentrantLock(); - private static int instances; // Factor for "proportional" control @@ -124,14 +120,9 @@ public class PIDController extends SendableBase { */ @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(); - } + m_Kp = Kp; + m_Ki = Ki; + m_Kd = Kd; } /** @@ -141,12 +132,7 @@ public class PIDController extends SendableBase { */ @SuppressWarnings("ParameterName") public void setP(double Kp) { - m_thisMutex.lock(); - try { - m_Kp = Kp; - } finally { - m_thisMutex.unlock(); - } + m_Kp = Kp; } /** @@ -156,12 +142,7 @@ public class PIDController extends SendableBase { */ @SuppressWarnings("ParameterName") public void setI(double Ki) { - m_thisMutex.lock(); - try { - m_Ki = Ki; - } finally { - m_thisMutex.unlock(); - } + m_Ki = Ki; } /** @@ -171,12 +152,7 @@ public class PIDController extends SendableBase { */ @SuppressWarnings("ParameterName") public void setD(double Kd) { - m_thisMutex.lock(); - try { - m_Kd = Kd; - } finally { - m_thisMutex.unlock(); - } + m_Kd = Kd; } /** @@ -185,12 +161,7 @@ public class PIDController extends SendableBase { * @return proportional coefficient */ public double getP() { - m_thisMutex.lock(); - try { - return m_Kp; - } finally { - m_thisMutex.unlock(); - } + return m_Kp; } /** @@ -199,12 +170,7 @@ public class PIDController extends SendableBase { * @return integral coefficient */ public double getI() { - m_thisMutex.lock(); - try { - return m_Ki; - } finally { - m_thisMutex.unlock(); - } + return m_Ki; } /** @@ -213,12 +179,7 @@ public class PIDController extends SendableBase { * @return differential coefficient */ public double getD() { - m_thisMutex.lock(); - try { - return m_Kd; - } finally { - m_thisMutex.unlock(); - } + return m_Kd; } /** @@ -238,12 +199,7 @@ public class PIDController extends SendableBase { * @return The latest calculated output. */ public double getOutput() { - m_thisMutex.lock(); - try { - return m_output; - } finally { - m_thisMutex.unlock(); - } + return m_output; } /** @@ -252,15 +208,10 @@ public class PIDController extends SendableBase { * @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(); + if (m_maximumInput > m_minimumInput) { + m_setpoint = clamp(setpoint, m_minimumInput, m_maximumInput); + } else { + m_setpoint = setpoint; } } @@ -270,18 +221,12 @@ public class PIDController extends SendableBase { * @return The current setpoint. */ public double getSetpoint() { - m_thisMutex.lock(); - try { - return m_setpoint; - } finally { - m_thisMutex.unlock(); - } + return m_setpoint; } /** - * 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. + * 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. * @@ -294,27 +239,21 @@ public class PIDController extends SendableBase { /** * Returns true if the error and change in error are below the specified tolerances. * - * @param tolerance The maximum allowable error. + * @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. + * @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(); + double deltaError = (error - m_prevError) / getPeriod(); - 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(); + 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; } } @@ -337,12 +276,7 @@ public class PIDController extends SendableBase { * @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(); - } + m_continuous = continuous; } /** @@ -352,18 +286,13 @@ public class PIDController extends SendableBase { * @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; + 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(); + // Clamp setpoint to new input + if (m_maximumInput > m_minimumInput) { + m_setpoint = clamp(m_setpoint, m_minimumInput, m_maximumInput); } } @@ -374,13 +303,8 @@ public class PIDController extends SendableBase { * @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(); - } + m_minimumOutput = minimumOutput; + m_maximumOutput = maximumOutput; } /** @@ -399,14 +323,9 @@ public class PIDController extends SendableBase { * @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(); - } + m_toleranceType = Tolerance.kAbsolute; + m_tolerance = tolerance; + m_deltaTolerance = deltaTolerance; } /** @@ -425,14 +344,9 @@ public class PIDController extends SendableBase { * @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(); - } + m_toleranceType = Tolerance.kPercent; + m_tolerance = tolerance; + m_deltaTolerance = deltaTolerance; } /** @@ -441,12 +355,7 @@ public class PIDController extends SendableBase { * @return The error. */ public double getError() { - m_thisMutex.lock(); - try { - return getContinuousError(m_currError); - } finally { - m_thisMutex.unlock(); - } + return getContinuousError(m_currError); } /** @@ -454,13 +363,19 @@ public class PIDController extends SendableBase { */ public double getDeltaError() { double error = getError(); + return (error - m_prevError) / getPeriod(); + } - 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. + * @param setpoint The new setpoint of the controller. + */ + public double calculate(double measurement, double setpoint) { + // Set setpoint to provided value + setSetpoint(setpoint); + return calculate(measurement); } /** @@ -469,48 +384,28 @@ public class PIDController extends SendableBase { * @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(); - } - } + m_prevError = m_currError; + m_currError = getContinuousError(m_setpoint - 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. - */ - 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(); + 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; } /** * 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(); - } + m_prevError = 0; + m_totalError = 0; + m_output = 0; } @Override @@ -524,7 +419,7 @@ public class PIDController extends SendableBase { /** * Wraps error around for continuous inputs. The original error is returned if continuous mode is - * disabled. This is an unsynchronized function. + * disabled. * * @param error The current error of the PID controller. * @return Error for continuous inputs. @@ -540,32 +435,9 @@ public class PIDController extends SendableBase { } } } - 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 deleted file mode 100644 index fa4b22a37c..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDControllerRunner.java +++ /dev/null @@ -1,134 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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 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(); - - // This is declared after all other member variables so that during - // PIDControllerRunner destruction, the Notifier is stopped before any member - // variables its callable uses are destructed. This avoids use-after-free - // bugs like crashes when locking is attempted on deallocated mutexes. - private final Notifier m_notifier = new Notifier(this::run); - - /** - * 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/PIDToleranceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java index 838aca4abc..97ab624d1a 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java @@ -12,14 +12,12 @@ 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_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; @@ -42,7 +40,6 @@ class PIDToleranceTest { void setUp() { m_inp = new FakeInput(); 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); } @@ -61,42 +58,53 @@ class PIDToleranceTest { void absoluteToleranceTest() { m_pidController.setAbsoluteTolerance(m_tolerance); m_pidController.setSetpoint(m_setpoint); - m_pidRunner.enable(); - Timer.delay(1); + for (int i = 0; i < 50; i++) { + m_pidController.calculate(m_inp.getMeasurement()); + } assertFalse(m_pidController.atSetpoint(), - "Error was in tolerance when it should not have been. Error was " - + m_pidController.getError()); + "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); + for (int i = 0; i < 50; i++) { + m_pidController.calculate(m_inp.getMeasurement()); + } assertTrue(m_pidController.atSetpoint(), - "Error was not in tolerance when it should have been. Error was " - + m_pidController.getError()); + "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); + for (int i = 0; i < 50; i++) { + m_pidController.calculate(m_inp.getMeasurement()); + } assertFalse(m_pidController.atSetpoint(), - "Error was in tolerance when it should not have been. Error was " - + m_pidController.getError()); + "Error was in tolerance when it should not have been. Error was " + m_pidController + .getError()); } @Test void percentToleranceTest() { m_pidController.setPercentTolerance(m_tolerance); m_pidController.setSetpoint(m_setpoint); - m_pidRunner.enable(); + for (int i = 0; i < 50; i++) { + m_pidController.calculate(m_inp.getMeasurement()); + } assertFalse(m_pidController.atSetpoint(), - "Error was in tolerance when it should not have been. Error was " - + m_pidController.getError()); + "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; - Timer.delay(1.0); + for (int i = 0; i < 50; i++) { + m_pidController.calculate(m_inp.getMeasurement()); + } assertTrue(m_pidController.atSetpoint(), - "Error was not in tolerance when it should have been. Error was " - + m_pidController.getError()); + "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; - Timer.delay(1.0); + for (int i = 0; i < 50; i++) { + m_pidController.calculate(m_inp.getMeasurement()); + } assertFalse(m_pidController.atSetpoint(), - "Error was in tolerance when it should not have been. Error was " - + m_pidController.getError()); + "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 0fbfcb0046..d981e0a845 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java @@ -20,7 +20,6 @@ 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.fixtures.MotorEncoderFixture; import edu.wpi.first.wpilibj.test.AbstractComsSetup; import edu.wpi.first.wpilibj.test.TestBench; @@ -68,8 +67,9 @@ public class MotorEncoderTest extends AbstractComsSetup { @Before public void setUp() { double initialSpeed = me.getMotor().get(); - assertTrue(me.getType() + " Did not start with an initial speed of 0 instead got: " - + initialSpeed, Math.abs(initialSpeed) < 0.001); + assertTrue( + me.getType() + " Did not start with an initial speed of 0 instead got: " + initialSpeed, + Math.abs(initialSpeed) < 0.001); me.setup(); } @@ -181,11 +181,12 @@ public class MotorEncoderTest extends AbstractComsSetup { pidController.setOutputRange(-0.2, 0.2); pidController.setSetpoint(1000); - PIDControllerRunner pidRunner = new PIDControllerRunner(pidController, - me.getEncoder()::getDistance, output -> me.getMotor().set(output)); - pidRunner.enable(); + Notifier pidRunner = new Notifier( + () -> me.getMotor().set(pidController.calculate(me.getEncoder().getDistance()))); + + pidRunner.startPeriodic(pidController.getPeriod()); Timer.delay(10.0); - pidRunner.disable(); + pidRunner.stop(); assertTrue( "PID loop did not reach reference within 10 seconds. The current error was" + pidController @@ -202,17 +203,15 @@ public class MotorEncoderTest extends AbstractComsSetup { pidController.setOutputRange(-0.3, 0.3); pidController.setSetpoint(600); - PIDControllerRunner pidRunner = new PIDControllerRunner(pidController, - () -> filter.calculate(me.getEncoder().getRate()), - output -> me.getMotor().set(output + 8e-5)); + Notifier pidRunner = + new Notifier(() -> me.getMotor().set(filter.calculate(me.getEncoder().getRate()) + 8e-5)); - pidRunner.enable(); + pidRunner.startPeriodic(pidController.getPeriod()); Timer.delay(10.0); - pidRunner.disable(); + pidRunner.stop(); - assertTrue( - "PID loop did not reach reference within 10 seconds. The error was: " - + pidController.getError(), pidController.atSetpoint()); + assertTrue("PID loop did not reach reference within 10 seconds. The error was: " + pidController + .getError(), pidController.atSetpoint()); pidController.close(); } @@ -233,8 +232,8 @@ public class MotorEncoderTest extends AbstractComsSetup { me.getCounters()[1].get(), 0); Timer.delay(0.5); // so this doesn't fail with the 0.5 second default // timeout on the encoders - assertTrue(me.getType() + " Encoder.getStopped() returned false after the motor was reset.", me - .getEncoder().getStopped()); + assertTrue(me.getType() + " Encoder.getStopped() returned false after the motor was reset.", + me.getEncoder().getStopped()); } } 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 39b4423cf3..4fed5191cf 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java @@ -24,17 +24,13 @@ 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; import edu.wpi.first.wpilibj.test.TestBench; -import static org.hamcrest.CoreMatchers.is; -import static org.hamcrest.CoreMatchers.not; import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertFalse; -import static org.junit.Assert.assertThat; import static org.junit.Assert.assertTrue; @@ -52,7 +48,6 @@ 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"}) @@ -85,8 +80,7 @@ public class PIDTest extends AbstractComsSetup { double ki = 0.0005; double kd = 0.0; for (int i = 0; i < 1; i++) { - data.addAll(Arrays.asList(new Object[][]{ - {kp, ki, kd, TestBench.getInstance().getTalonPair()}, + data.addAll(Arrays.asList(new Object[][]{{kp, ki, kd, TestBench.getInstance().getTalonPair()}, {kp, ki, kd, TestBench.getInstance().getVictorPair()}, {kp, ki, kd, TestBench.getInstance().getJaguarPair()}})); } @@ -112,18 +106,14 @@ public class PIDTest extends AbstractComsSetup { m_builder = new SendableBuilderImpl(); m_builder.setTable(m_table); 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() { logger.fine("Teardown: " + me.getType()); - m_runner.disable(); m_controller.close(); m_controller = null; - m_runner = null; me.reset(); } @@ -141,9 +131,8 @@ public class PIDTest extends AbstractComsSetup { setupOutputRange(); 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); + assertEquals("PID.getError() did not start at " + reference, reference, m_controller.getError(), + 0); m_builder.updateTable(); assertEquals(k_p, m_table.getEntry("Kp").getDouble(9999999), 0); assertEquals(k_i, m_table.getEntry("Ki").getDouble(9999999), 0); @@ -152,34 +141,13 @@ public class PIDTest extends AbstractComsSetup { assertFalse(m_table.getEntry("enabled").getBoolean(true)); } - @Test - public void testRestartAfterEnable() { - setupAbsoluteTolerance(); - setupOutputRange(); - double reference = 2500.0; - m_controller.setSetpoint(reference); - m_runner.enable(); - m_builder.updateTable(); - assertTrue(m_table.getEntry("enabled").getBoolean(false)); - 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_runner.isEnabled()); - assertEquals(0, me.getMotor().get(), 0); - } - @Test public void testSetSetpoint() { setupAbsoluteTolerance(); setupOutputRange(); double reference = 2500.0; - m_runner.disable(); m_controller.setSetpoint(reference); - m_runner.enable(); - assertEquals("Did not correctly set reference", reference, m_controller - .getSetpoint(), 1e-3); + assertEquals("Did not correctly set reference", reference, m_controller.getSetpoint(), 1e-3); } @Test(timeout = 10000) @@ -191,9 +159,11 @@ public class PIDTest extends AbstractComsSetup { m_controller.setSetpoint(reference); assertEquals(pidData() + "did not have an error of " + reference, reference, m_controller.getError(), 0); - m_runner.enable(); + Notifier pidRunner = new Notifier( + () -> me.getMotor().set(m_controller.calculate(me.getEncoder().getDistance()))); + pidRunner.startPeriodic(m_controller.getPeriod()); Timer.delay(5); - m_runner.disable(); + pidRunner.stop(); assertTrue(pidData() + "Was not on Target. Controller Error: " + m_controller.getError(), m_controller.atSetpoint()); }