mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
62be0392b6
std::scoped_lock was introduced in C++17 and is strictly better than std::lock_guard as it supports locking any number of mutexes safely. It's also easier to use than std::lock for locking multiple mutexes at once.
73 lines
2.4 KiB
C++
73 lines
2.4 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* 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();
|
|
}
|
|
});
|
|
}
|