forgot new files

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 <glm/glm.hpp>
+#include <QDebug>
+#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;
+}

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 <limits>
+#include <QVector>
+
+// 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<float>::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<Row> _history{};
+    QString _label{ "" };
+
+};
+
+#endif // hifi_PIDController_h