pidcontroller.h

Go to the documentation of this file.

 
#pragma once
 
 
#include <promeki/config.h>
#if PROMEKI_ENABLE_CORE
#include <promeki/function.h>
#include <promeki/namespace.h>
#include <functional>
 
PROMEKI_NAMESPACE_BEGIN
 
template <typename ValueType = double, typename TimeType = double> class PIDController {
        public:
                using CurrentTimeFunc = Function<TimeType()>;
                using CurrentValueFunc = Function<ValueType()>;
 
                PIDController(CurrentValueFunc currentValueFunc, CurrentTimeFunc currentTimeFunc)
                    : _gainP(1), _gainI(0), _gainD(0), _integral(0), _prevError(0), _setPoint(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
 
#endif // PROMEKI_ENABLE_CORE