pidcontroller.h

Go to the documentation of this file.

 
#pragma once
 
#include <promeki/core/namespace.h>
#include <functional>
 
PROMEKI_NAMESPACE_BEGIN
 
template <typename ValueType = double, typename TimeType = double>
class PIDController {
 public:
     using CurrentTimeFunc = std::function<TimeType()>;
     using CurrentValueFunc = std::function<ValueType()>;
 
     PIDController(CurrentValueFunc currentValueFunc, CurrentTimeFunc currentTimeFunc)
         : _gainP(1), _gainI(0), _gainD(0),
           _setPoint(0), _integral(0), _prevError(0),
           _currentValue(currentValueFunc), _currentTime(currentTimeFunc),
           _prevUpdate(_currentTime()) {}
 
     ValueType getGainP() const { return _gainP; }
 
     void setGainP(const ValueType &val) { _gainP = val; }
 
     ValueType getGainI() const { return _gainI; }
 
     void setGainI(const ValueType &val) { _gainI = val; }
 
     ValueType getGainD() const { return _gainD; }
 
     void setGainD(const ValueType &val) { _gainD = val; }
 
     void updateSetPoint(const ValueType &val) { _setPoint = val; }
 
     ValueType step() {
         TimeType now = _currentTime();
         TimeType dt = now - _prevUpdate;
         _prevUpdate = now;
 
         ValueType pv = _currentValue();
         ValueType error = _setPoint - pv;
         _integral += error * dt;
         ValueType derivative = (error - _prevError) / dt;
         _prevError = error;
 
         return _gainP * error + _gainI * _integral + _gainD * derivative;
     }
 
     void reset() {
         _integral = {};
         _prevError = {};
         _prevUpdate = _currentTime();
         return;
     }
 
private:
     ValueType _gainP = 1;
     ValueType _gainI = 1;
     ValueType _gainD = 1;
     ValueType _integral = 0;
     ValueType _prevError = 0;
     ValueType _setPoint = 0;
     CurrentValueFunc _currentValue;
     CurrentTimeFunc _currentTime;
     TimeType _prevUpdate = 0;
};
 
 
PROMEKI_NAMESPACE_END