Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Remove PIDControllerRunner and mutex from new PIDController (#1795)

Browse Source 
Teams that wish to use it asynchronously may still do so - they simply need to handle the thread safety themselves (it is not that difficult, and can be done more cleanly in the calling code anyway).
This commit is contained in:
Oblarg
2019-08-04 03:01:11 -04:00
committed by Peter Johnson
parent 98d0706de8
commit c9873e81b2
11 changed files with 165 additions and 706 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
wpilibc/src/main/native/cpp/controller/PIDController.cpp
View File

@@ -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);

72
wpilibc/src/main/native/cpp/controller/PIDControllerRunner.cpp
View File

@@ -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<double(void)> measurementSource,
std::function<void(double)> 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();
}
});
}

21
wpilibc/src/main/native/include/frc/controller/PIDController.h
View File

@@ -10,8 +10,6 @@
#include <functional>
#include <limits>
#include <wpi/mutex.h>
#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

78
wpilibc/src/main/native/include/frc/controller/PIDControllerRunner.h
View File

@@ -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 <functional>
#include <wpi/mutex.h>
#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<double(void)> measurementSource,
std::function<void(double)> 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<double(void)> m_measurementSource;
std::function<void(double)> 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

28
wpilibc/src/test/native/cpp/PIDToleranceTest.cpp
View File

@@ -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 "

24
wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
View File

@@ -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())

266
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
View File

@@ -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.
*
* <p>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.
*
* <p>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));
}

134
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDControllerRunner.java
View File

@@ -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.
*
* <p>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();
}
});
}
}

52
wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java
View File

@@ -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());
}
}

33
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
View File

@@ -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());
}
}

46
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java
View File

@@ -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());
}