Diff: pidLo/pidLo.cpp

Revision:

0:62ded4362bbc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pidLo/pidLo.cpp	Tue Nov 23 10:35:12 2021 +0000
@@ -0,0 +1,42 @@
+#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;
+}
\ No newline at end of file