54 lines
1.3 KiB
C++
54 lines
1.3 KiB
C++
#include "pid.hpp"
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
|
|
PID::PID(float kp, float ki, float kd, float target,
|
|
PIDMode mode, float out_limit, float int_limit)
|
|
: _kp(kp), _ki(ki), _kd(kd), _target(target), _mode(mode)
|
|
, _out_limit(out_limit), _int_limit(int_limit)
|
|
, _output(0), _integral(0), _prev_error(0), _last_measured(0), _first(true) {}
|
|
|
|
void PID::setPID(float kp, float ki, float kd)
|
|
{
|
|
_kp = kp; _ki = ki; _kd = kd;
|
|
reset();
|
|
}
|
|
|
|
void PID::reset()
|
|
{
|
|
_integral = 0;
|
|
_prev_error = 0;
|
|
_output = 0;
|
|
_first = true;
|
|
}
|
|
|
|
float PID::update(float measured)
|
|
{
|
|
float error = _target - measured;
|
|
|
|
if (_mode == PID_POSITIONAL)
|
|
{
|
|
_integral += error;
|
|
_integral = std::clamp(_integral, -_int_limit, _int_limit);
|
|
|
|
float derivative = 0;
|
|
if (!_first) derivative = error - _prev_error;
|
|
|
|
_output = _kp * error + _ki * _integral + _kd * derivative;
|
|
}
|
|
else // PID_INCREMENTAL
|
|
{
|
|
float inc = _kp * (error - _prev_error) + _ki * error;
|
|
if (!_first)
|
|
inc += _kd * (error - 2.0f * _prev_error + (_prev_error - (_target - _last_measured)));
|
|
_output += inc;
|
|
}
|
|
|
|
_output = std::clamp(_output, -_out_limit, _out_limit);
|
|
_prev_error = error;
|
|
_last_measured = measured;
|
|
_first = false;
|
|
|
|
return _output;
|
|
}
|