File content as of revision 3:aef2dcb63762:

#include "pidLo.h"

pidLo::pidLo(float p, float i, float d, float _Ts, float _max_out, float _vff, float _freq_lpf, float _max_integral){
pidLo::setTunings(p, i, d);
ts = _Ts;
max_out = _max_out;
vff = _vff;
freq_lpf = _freq_lpf;
max_integral = _max_integral;
last_error = 0;
integ = 0;
deriv = 0;
}

void pidLo::setTunings(float p, float i, float d){
kp = p;
ki = i;
kd = d;
}

float pidLo :: createpwm(float setpoint, float feedback, float max){
error = setpoint - feedback;

// get integral (bilinear transform)
integ += (error + last_error)*ts/2;
if (fabs(integ) > fabs(max_integral)){
integ = fabs(max_integral)*(fabs(integ)/integ);
}

// get derivatif with freq_lpf (bilinear transform)
// y(n) = (2*(x(n)-x(n-1) - (y(n-1)(t-2N)))) / (t+2N)
deriv = (error - last_error)/ts;


last_error = error;
last_out = kp*error + ki*integ + kd*deriv + vff*setpoint;
if (fabs(last_out) > fabs(max_out)){
last_out = fabs(max_out)*(fabs(last_out)/last_out);
}

return last_out;
}