KRAI ITB GARUDAGO / Mbed 2 deprecated frictionwheel

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

pidLo/pidLo.cpp@3:aef2dcb63762, 2021-12-17 (annotated)

Committer:
kelvinsutirta
Date:
Fri Dec 17 08:53:50 2021 +0000
Revision:
3:aef2dcb63762
Parent:
0:62ded4362bbc 
with PID;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
putriliza 0:62ded4362bbc 1 #include "pidLo.h"
putriliza 0:62ded4362bbc 2
putriliza 0:62ded4362bbc 3 pidLo::pidLo(float p, float i, float d, float _Ts, float _max_out, float _vff, float _freq_lpf, float _max_integral){
putriliza 0:62ded4362bbc 4 pidLo::setTunings(p, i, d);
putriliza 0:62ded4362bbc 5 ts = _Ts;
putriliza 0:62ded4362bbc 6 max_out = _max_out;
putriliza 0:62ded4362bbc 7 vff = _vff;
putriliza 0:62ded4362bbc 8 freq_lpf = _freq_lpf;
putriliza 0:62ded4362bbc 9 max_integral = _max_integral;
putriliza 0:62ded4362bbc 10 last_error = 0;
putriliza 0:62ded4362bbc 11 integ = 0;
putriliza 0:62ded4362bbc 12 deriv = 0;
putriliza 0:62ded4362bbc 13 }
putriliza 0:62ded4362bbc 14
putriliza 0:62ded4362bbc 15 void pidLo::setTunings(float p, float i, float d){
putriliza 0:62ded4362bbc 16 kp = p;
putriliza 0:62ded4362bbc 17 ki = i;
putriliza 0:62ded4362bbc 18 kd = d;
putriliza 0:62ded4362bbc 19 }
putriliza 0:62ded4362bbc 20
putriliza 0:62ded4362bbc 21 float pidLo :: createpwm(float setpoint, float feedback, float max){
putriliza 0:62ded4362bbc 22 error = setpoint - feedback;
putriliza 0:62ded4362bbc 23
putriliza 0:62ded4362bbc 24 // get integral (bilinear transform)
putriliza 0:62ded4362bbc 25 integ += (error + last_error)*ts/2;
putriliza 0:62ded4362bbc 26 if (fabs(integ) > fabs(max_integral)){
putriliza 0:62ded4362bbc 27 integ = fabs(max_integral)*(fabs(integ)/integ);
putriliza 0:62ded4362bbc 28 }
putriliza 0:62ded4362bbc 29
putriliza 0:62ded4362bbc 30 // get derivatif with freq_lpf (bilinear transform)
putriliza 0:62ded4362bbc 31 // y(n) = (2*(x(n)-x(n-1) - (y(n-1)(t-2N)))) / (t+2N)
putriliza 0:62ded4362bbc 32 deriv = (error - last_error)/ts;
putriliza 0:62ded4362bbc 33
putriliza 0:62ded4362bbc 34
putriliza 0:62ded4362bbc 35 last_error = error;
putriliza 0:62ded4362bbc 36 last_out = kp*error + ki*integ + kd*deriv + vff*setpoint;
putriliza 0:62ded4362bbc 37 if (fabs(last_out) > fabs(max_out)){
putriliza 0:62ded4362bbc 38 last_out = fabs(max_out)*(fabs(last_out)/last_out);
putriliza 0:62ded4362bbc 39 }
putriliza 0:62ded4362bbc 40
putriliza 0:62ded4362bbc 41 return last_out;
putriliza 0:62ded4362bbc 42 }