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