////  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 "PIDController.h" #include <glm/glm.hpp> #include <QDebug> #include "SharedLogging.h" float PIDController::update(float measuredValue, float dt, bool resetAccumulator) {    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,        getControlledValueLowLimit(),        getControlledValueHighLimit());     if (getIsLogging()) {  // if logging/reporting        updateHistory(measuredValue, dt, error, accumulatedError, changeInError, p, i, d, computedValue);    }    Q_ASSERT(!glm::isnan(computedValue));     // update state for next time    _lastError = error;    _lastAccumulation = accumulatedError;    return computedValue;} // Just for logging/reporting. Used when picking/verifying the operational parameters.void PIDController::updateHistory(float measuredValue, float dt, float error, float accumulatedError, float changeInError, float p, float i, float d, float computedValue) {    // 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.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        reportHistory();        _history.resize(0);    }}void PIDController::reportHistory() {    qCDebug(shared) << _label << "measured dt || 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.error << row.accumulated << row.changed <<            "||" << row.p << row.i << row.d << row.computed << 1.0f/row.computed;    }    qCDebug(shared) << "Limits: setpoint" << getMeasuredValueSetpoint() << "accumulate" << getAccumulatedValueLowLimit() << getAccumulatedValueHighLimit() <<        "controlled" << getControlledValueLowLimit() << getControlledValueHighLimit() <<        "kp/ki/kd" << getKP() << getKI() << getKD();}