fachrizi kiki / PID_lib

library to use pid easier

PID_lib.cpp@2:77ef3d60d8d9, 23 months ago (annotated)

Committer:
fachrizi_kiki
Date:
Sun Oct 16 11:14:26 2022 +0000
Revision:
2:77ef3d60d8d9
Parent:
1:ebb9cfc0cff5
Child:
3:80e7ed9fdb02
PID library for use control method easier

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fachrizi_kiki 0:e14308f43fdf 1 #include "PID_lib.h"
fachrizi_kiki 2:77ef3d60d8d9 2
fachrizi_kiki 0:e14308f43fdf 3
fachrizi_kiki 2:77ef3d60d8d9 4 PID_lib::PID_lib(PinName direksi1, PinName direksi2, PinName pulseWidth) : dir1(direksi1), dir2(direksi2), pwm(pulseWidth){
fachrizi_kiki 1:ebb9cfc0cff5 5 rpm = rpm;
fachrizi_kiki 0:e14308f43fdf 6 dir1 = 0;
fachrizi_kiki 0:e14308f43fdf 7 dir2 = 0;
fachrizi_kiki 1:ebb9cfc0cff5 8 pwm = 0;
fachrizi_kiki 0:e14308f43fdf 9
fachrizi_kiki 0:e14308f43fdf 10 float ppr = 540.0f;
fachrizi_kiki 0:e14308f43fdf 11 float phi = 3.14285714;
fachrizi_kiki 0:e14308f43fdf 12
fachrizi_kiki 0:e14308f43fdf 13 t.start();
fachrizi_kiki 0:e14308f43fdf 14 t.reset();
fachrizi_kiki 0:e14308f43fdf 15 }
fachrizi_kiki 0:e14308f43fdf 16
fachrizi_kiki 0:e14308f43fdf 17
fachrizi_kiki 0:e14308f43fdf 18
fachrizi_kiki 0:e14308f43fdf 19 // kp ki kd untuk sp 50 tuning manual lagi stelah tuning matlab
fachrizi_kiki 0:e14308f43fdf 20
fachrizi_kiki 0:e14308f43fdf 21
fachrizi_kiki 0:e14308f43fdf 22
fachrizi_kiki 2:77ef3d60d8d9 23 void PID_lib::pid_pwm(float target, float kp_,float ki_,float kd_, float rpm){
fachrizi_kiki 0:e14308f43fdf 24 tim = t;
fachrizi_kiki 0:e14308f43fdf 25 dt = tim - lastime;
fachrizi_kiki 0:e14308f43fdf 26
fachrizi_kiki 0:e14308f43fdf 27 rpmFilt = 0.03046875*rpm + 0.03046875*rpmn1 + 0.93906251*rpmFiltn1;//10hz filter
fachrizi_kiki 0:e14308f43fdf 28
fachrizi_kiki 0:e14308f43fdf 29 //error computing start
fachrizi_kiki 2:77ef3d60d8d9 30 e = target - rpmFilt;
fachrizi_kiki 0:e14308f43fdf 31 eI += e;
fachrizi_kiki 0:e14308f43fdf 32 eD = e - laste;
fachrizi_kiki 0:e14308f43fdf 33 //error computing end
fachrizi_kiki 0:e14308f43fdf 34
fachrizi_kiki 0:e14308f43fdf 35 //storing error value start
fachrizi_kiki 0:e14308f43fdf 36 hP = e*kp_;
fachrizi_kiki 0:e14308f43fdf 37 hI = eI*ki_*dt;
fachrizi_kiki 0:e14308f43fdf 38 hD = eD*kd_/dt;
fachrizi_kiki 0:e14308f43fdf 39 //storing error value end
fachrizi_kiki 0:e14308f43fdf 40
fachrizi_kiki 0:e14308f43fdf 41 //saturasu ki start
fachrizi_kiki 0:e14308f43fdf 42 if(hI < 0.5 && hI > 0){
fachrizi_kiki 0:e14308f43fdf 43 setI = hI;
fachrizi_kiki 0:e14308f43fdf 44 }else if(hI > 0.5){
fachrizi_kiki 0:e14308f43fdf 45 setI = 0.5;
fachrizi_kiki 0:e14308f43fdf 46 }
fachrizi_kiki 0:e14308f43fdf 47 else if(hI < 0 && hI > -0.5){
fachrizi_kiki 0:e14308f43fdf 48 setI = hI;
fachrizi_kiki 0:e14308f43fdf 49 }else if(hI < -0.5){
fachrizi_kiki 0:e14308f43fdf 50 setI = -0.5;
fachrizi_kiki 0:e14308f43fdf 51 }
fachrizi_kiki 0:e14308f43fdf 52 //saturasi ki end
fachrizi_kiki 0:e14308f43fdf 53
fachrizi_kiki 0:e14308f43fdf 54 pidPwm = hP+setI+hD;//pwm pid
fachrizi_kiki 0:e14308f43fdf 55 lastime = tim;//update timer
fachrizi_kiki 0:e14308f43fdf 56 laste = e;//update error
fachrizi_kiki 0:e14308f43fdf 57
fachrizi_kiki 0:e14308f43fdf 58 rpmFiltn1 = rpmFilt;rpmn1 = rpm;//update filter
fachrizi_kiki 0:e14308f43fdf 59 //motor direksi start
fachrizi_kiki 0:e14308f43fdf 60 if(pidPwm < 0){dir1 = 1; dir2 = 0;}
fachrizi_kiki 0:e14308f43fdf 61 else{dir1 = 0; dir2 = 1;}
fachrizi_kiki 0:e14308f43fdf 62 //motor direksi end
fachrizi_kiki 0:e14308f43fdf 63
fachrizi_kiki 0:e14308f43fdf 64 //pwm saturasi start
fachrizi_kiki 0:e14308f43fdf 65 if(pidPwm > 1){
fachrizi_kiki 0:e14308f43fdf 66 pwmLebih = 1;
fachrizi_kiki 0:e14308f43fdf 67 }else if(pidPwm < 1 && pidPwm > 0){
fachrizi_kiki 0:e14308f43fdf 68 pwmLebih = fabs(pidPwm);
fachrizi_kiki 0:e14308f43fdf 69 }else if(pidPwm < -1){
fachrizi_kiki 0:e14308f43fdf 70 pwmLebih = -1;
fachrizi_kiki 0:e14308f43fdf 71 }else if(pidPwm > -1 && pidPwm < 0){
fachrizi_kiki 0:e14308f43fdf 72 pwmLebih = fabs(pidPwm);
fachrizi_kiki 0:e14308f43fdf 73 }
fachrizi_kiki 0:e14308f43fdf 74 //pwm saturasi end
fachrizi_kiki 0:e14308f43fdf 75
fachrizi_kiki 0:e14308f43fdf 76 //motor start
fachrizi_kiki 2:77ef3d60d8d9 77 pwm = fabs(pwmLebih);
fachrizi_kiki 0:e14308f43fdf 78 //motor end
fachrizi_kiki 0:e14308f43fdf 79
fachrizi_kiki 0:e14308f43fdf 80 lastPid = pidPwm;
fachrizi_kiki 2:77ef3d60d8d9 81 printf("%f\n", pwmLebih);
fachrizi_kiki 0:e14308f43fdf 82 }
fachrizi_kiki 0:e14308f43fdf 83