From 97898a1ad1a4ce6972c52fe5dbf1736a5b34acc6 Mon Sep 17 00:00:00 2001 From: Howard Stearns Date: Tue, 17 Nov 2015 20:43:40 -0800 Subject: [PATCH] forgot new files --- libraries/shared/src/PIDController.cpp | 71 ++++++++++++++++++++ libraries/shared/src/PIDController.h | 91 ++++++++++++++++++++++++++ 2 files changed, 162 insertions(+) create mode 100644 libraries/shared/src/PIDController.cpp create mode 100644 libraries/shared/src/PIDController.h diff --git a/libraries/shared/src/PIDController.cpp b/libraries/shared/src/PIDController.cpp new file mode 100644 index 0000000000..42447e9809 --- /dev/null +++ b/libraries/shared/src/PIDController.cpp @@ -0,0 +1,71 @@ +// +// PIDController.cpp +// libraries/shared/src +// +// Created by Howard Stearns 11/13/15. +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include +#include +#include "SharedLogging.h" +#include "PIDController.h" + +float PIDController::update(float measuredValue, float dt, bool resetAccumulator, float FIXME1, float FIXME2) { + const float error = getMeasuredValueSetpoint() - measuredValue; // Sign is the direction we want measuredValue to go. Positive means go higher. + + const float p = getKP() * error; // term is Proportional to error + + const float accumulatedError = glm::clamp(error * dt + (resetAccumulator ? 0 : _lastAccumulation), // integrate error + getAccumulatedValueLowLimit(), // but clamp by anti-windup limits + getAccumulatedValueHighLimit()); + const float i = getKI() * accumulatedError; // term is Integral of error + + const float changeInError = (error - _lastError) / dt; // positive value denotes increasing deficit + const float d = getKD() * changeInError; // term is Derivative of Error + + const float computedValue = glm::clamp(p + i + d + getBias(), + getControlledValueLowLimit(), + getControlledValueHighLimit()); + + if (_history.capacity()) { // THIS SECTION ONLY FOR LOGGING + // Don't report each update(), as the I/O messes with the results a lot. + // Instead, add to history, and then dump out at once when full. + // Typically, the first few values reported in each batch should be ignored. + const int n = _history.size(); + _history.resize(n + 1); + Row& next = _history[n]; + next.measured = measuredValue; + next.FIXME1 = FIXME1; + next.FIXME2 = FIXME2; + next.dt = dt; + next.error = error; + next.accumulated = accumulatedError; + next.changed = changeInError; + next.p = p; + next.i = i; + next.d = d; + next.computed = computedValue; + if (_history.size() == _history.capacity()) { // report when buffer is full + qCDebug(shared) << _label << "measured dt FIXME || error accumulated changed || p i d controlled"; + for (int i = 0; i < _history.size(); i++) { + Row& row = _history[i]; + qCDebug(shared) << row.measured << row.dt << row.FIXME1 << row.FIXME2 << + "||" << row.error << row.accumulated << row.changed << + "||" << row.p << row.i << row.d << row.computed; + } + qCDebug(shared) << "Limits: accumulate" << getAccumulatedValueLowLimit() << getAccumulatedValueHighLimit() << + "controlled" << getControlledValueLowLimit() << getControlledValueHighLimit() << + "kp/ki/kd/bias" << getKP() << getKI() << getKD() << getBias(); + _history.resize(0); + } + } + + // update state for next time + _lastError = error; + _lastAccumulation = accumulatedError; + return computedValue; +} \ No newline at end of file diff --git a/libraries/shared/src/PIDController.h b/libraries/shared/src/PIDController.h new file mode 100644 index 0000000000..dd0c4a8528 --- /dev/null +++ b/libraries/shared/src/PIDController.h @@ -0,0 +1,91 @@ +// +// PIDController.h +// libraries/shared/src +// +// Given a measure of system performance (such as frame rate, where bigger denotes more system work), +// compute a value that the system can take as input to control the amount of work done (such as an 1/LOD-distance, +// where bigger tends to give a higher measured system performance value). The controller's job is to compute a +// controlled value such that the measured value stays near the specified setpoint, even as system load changes. +// See http://www.wetmachine.com/inventing-the-future/mostly-reliable-performance-of-software-processes-by-dynamic-control-of-quality-parameters/ +// +// Created by Howard Stearns 11/13/15. +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_PIDController_h +#define hifi_PIDController_h + +#include +#include + +// Although our coding standard shuns abbreviations, the control systems literature pretty uniformly uses p, i, d, and dt rather than +// proportionalTerm, integralTerm, derivativeTerm, and deltaTime. Here we will be consistent with the literature. +class PIDController { + +public: + // These three are the main interface: + void setMeasuredValueSetpoint(float newValue) { _measuredValueSetpoint = newValue; } + float update(float measuredValue, float dt, bool resetAccumulator = false, float FIXME1 = 0.0f, float FIXME2 = 0.0f); // returns the new computedValue + void setHistorySize(QString label = QString(""), int size = 0) { _history.reserve(size); _history.resize(0); _label = label; } // non-empty does logging + + // There are several values that rarely change and might be thought of as "constants", but which do change during tuning, debugging, or other + // special-but-expected circumstances. Thus the instance vars are not const. + float getMeasuredValueSetpoint() const { return _measuredValueSetpoint; } + // In normal operation (where we can easily reach setpoint), controlledValue is typcially pinned at max. + // Defaults to [0, max float], but for 1/LODdistance, it might be, say, [0, 0.2 or 0.1] + float getControlledValueLowLimit() const { return _controlledValueLowLimit; } + float getControlledValueHighLimit() const { return _controlledValueHighLimit; } + float getAntiWindupFactor() const { return _antiWindupFactor; } // default 10 + float getKP() const { return _kp; } + float getKI() const { return _ki; } + float getKD() const { return _kd; } + float getBias() const { return _bias; } + float getAccumulatedValueHighLimit() const { return getAntiWindupFactor() * getMeasuredValueSetpoint(); } + float getAccumulatedValueLowLimit() const { return -getAntiWindupFactor() * getMeasuredValueSetpoint(); } + + void setControlledValueLowLimit(float newValue) { _controlledValueLowLimit = newValue; } + void setControlledValueHighLimit(float newValue) { _controlledValueHighLimit = newValue; } + void setAntiWindupFactor(float newValue) { _antiWindupFactor = newValue; } + void setKP(float newValue) { _kp = newValue; } + void setKI(float newValue) { _ki = newValue; } + void setKD(float newValue) { _kd = newValue; } + void setBias(float newValue) { _bias = newValue; } + + class Row { // one row of accumulated history, used only for logging (if at all) + public: + float FIXME1; + float FIXME2; + float measured; + float dt; + float error; + float accumulated; + float changed; + float p; + float i; + float d; + float bias; + float computed; + }; +protected: + float _measuredValueSetpoint { 0.0f }; + float _controlledValueLowLimit { 0.0f }; + float _controlledValueHighLimit { std::numeric_limits::max() }; + float _antiWindupFactor { 10.0f }; + float _kp { 0.0f }; + float _ki { 0.0f }; + float _kd { 0.0f }; + float _bias { 0.0f }; + + // Controller state + float _lastError{ 0.0f }; + float _lastAccumulation{ 0.0f }; + // reporting + QVector _history{}; + QString _label{ "" }; + +}; + +#endif // hifi_PIDController_h