fachrizi kiki / PID_lib

library to use pid easier

PID_lib.cpp@0:e14308f43fdf, 20 months ago (annotated)

Committer:
fachrizi_kiki
Date:
Sun Oct 16 09:27:54 2022 +0000
Revision:
0:e14308f43fdf
Child:
1:ebb9cfc0cff5
try to publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fachrizi_kiki 0:e14308f43fdf 1 #include "PID_lib.h"
fachrizi_kiki 0:e14308f43fdf 2 #include "mbed.h"
fachrizi_kiki 0:e14308f43fdf 3
fachrizi_kiki 0:e14308f43fdf 4 PID_lib::PID_lib(PinName direksi1, PinName direksi2, PinName pulseWidth, float rpm) : dir1(direksi1), dir2(direksi2), pwm_(pulseWidth), rpm_(rpm){
fachrizi_kiki 0:e14308f43fdf 5 rpm = rpm_;
fachrizi_kiki 0:e14308f43fdf 6 dir1 = 0;
fachrizi_kiki 0:e14308f43fdf 7 dir2 = 0;
fachrizi_kiki 0:e14308f43fdf 8 pwm_ = 0;
fachrizi_kiki 0:e14308f43fdf 9
fachrizi_kiki 0:e14308f43fdf 10 float kpA = 0.007770;
fachrizi_kiki 0:e14308f43fdf 11 float kiA = 0.003297;
fachrizi_kiki 0:e14308f43fdf 12 float kdA = 0.000015;
fachrizi_kiki 0:e14308f43fdf 13
fachrizi_kiki 0:e14308f43fdf 14 float kpB = 0.03134;
fachrizi_kiki 0:e14308f43fdf 15 float kiB = 0.003605;
fachrizi_kiki 0:e14308f43fdf 16 float kdB = 0.001002;
fachrizi_kiki 0:e14308f43fdf 17
fachrizi_kiki 0:e14308f43fdf 18 float kpC = 0.02040;
fachrizi_kiki 0:e14308f43fdf 19 float kiC = 0.003292;
fachrizi_kiki 0:e14308f43fdf 20 float kdC = 0.000015;
fachrizi_kiki 0:e14308f43fdf 21
fachrizi_kiki 0:e14308f43fdf 22 float kpD = 0.02031;
fachrizi_kiki 0:e14308f43fdf 23 float kiD = 0.002760;
fachrizi_kiki 0:e14308f43fdf 24 float kdD = 3.943e-04;
fachrizi_kiki 0:e14308f43fdf 25
fachrizi_kiki 0:e14308f43fdf 26 float ppr = 540.0f;
fachrizi_kiki 0:e14308f43fdf 27 float phi = 3.14285714;
fachrizi_kiki 0:e14308f43fdf 28
fachrizi_kiki 0:e14308f43fdf 29 t.start();
fachrizi_kiki 0:e14308f43fdf 30 t.reset();
fachrizi_kiki 0:e14308f43fdf 31 }
fachrizi_kiki 0:e14308f43fdf 32
fachrizi_kiki 0:e14308f43fdf 33
fachrizi_kiki 0:e14308f43fdf 34
fachrizi_kiki 0:e14308f43fdf 35 // kp ki kd untuk sp 50 tuning manual lagi stelah tuning matlab
fachrizi_kiki 0:e14308f43fdf 36
fachrizi_kiki 0:e14308f43fdf 37
fachrizi_kiki 0:e14308f43fdf 38
fachrizi_kiki 0:e14308f43fdf 39 void pid_pwm(float target, float kp_,float ki_,float kd_){
fachrizi_kiki 0:e14308f43fdf 40 tim = t;
fachrizi_kiki 0:e14308f43fdf 41 dt = tim - lastime;
fachrizi_kiki 0:e14308f43fdf 42
fachrizi_kiki 0:e14308f43fdf 43 rpmFilt = 0.03046875*rpm + 0.03046875*rpmn1 + 0.93906251*rpmFiltn1;//10hz filter
fachrizi_kiki 0:e14308f43fdf 44
fachrizi_kiki 0:e14308f43fdf 45 //error computing start
fachrizi_kiki 0:e14308f43fdf 46 e = sp - rpmFilt;
fachrizi_kiki 0:e14308f43fdf 47 eI += e;
fachrizi_kiki 0:e14308f43fdf 48 eD = e - laste;
fachrizi_kiki 0:e14308f43fdf 49 //error computing end
fachrizi_kiki 0:e14308f43fdf 50
fachrizi_kiki 0:e14308f43fdf 51 //storing error value start
fachrizi_kiki 0:e14308f43fdf 52 hP = e*kp_;
fachrizi_kiki 0:e14308f43fdf 53 hI = eI*ki_*dt;
fachrizi_kiki 0:e14308f43fdf 54 hD = eD*kd_/dt;
fachrizi_kiki 0:e14308f43fdf 55 //storing error value end
fachrizi_kiki 0:e14308f43fdf 56
fachrizi_kiki 0:e14308f43fdf 57 //saturasu ki start
fachrizi_kiki 0:e14308f43fdf 58 if(hI < 0.5 && hI > 0){
fachrizi_kiki 0:e14308f43fdf 59 setI = hI;
fachrizi_kiki 0:e14308f43fdf 60 }else if(hI > 0.5){
fachrizi_kiki 0:e14308f43fdf 61 setI = 0.5;
fachrizi_kiki 0:e14308f43fdf 62 }
fachrizi_kiki 0:e14308f43fdf 63 else if(hI < 0 && hI > -0.5){
fachrizi_kiki 0:e14308f43fdf 64 setI = hI;
fachrizi_kiki 0:e14308f43fdf 65 }else if(hI < -0.5){
fachrizi_kiki 0:e14308f43fdf 66 setI = -0.5;
fachrizi_kiki 0:e14308f43fdf 67 }
fachrizi_kiki 0:e14308f43fdf 68 //saturasi ki end
fachrizi_kiki 0:e14308f43fdf 69
fachrizi_kiki 0:e14308f43fdf 70 pidPwm = hP+setI+hD;//pwm pid
fachrizi_kiki 0:e14308f43fdf 71 lastime = tim;//update timer
fachrizi_kiki 0:e14308f43fdf 72 laste = e;//update error
fachrizi_kiki 0:e14308f43fdf 73
fachrizi_kiki 0:e14308f43fdf 74 rpmFiltn1 = rpmFilt;rpmn1 = rpm;//update filter
fachrizi_kiki 0:e14308f43fdf 75 //motor direksi start
fachrizi_kiki 0:e14308f43fdf 76 if(pidPwm < 0){dir1 = 1; dir2 = 0;}
fachrizi_kiki 0:e14308f43fdf 77 else{dir1 = 0; dir2 = 1;}
fachrizi_kiki 0:e14308f43fdf 78 //motor direksi end
fachrizi_kiki 0:e14308f43fdf 79
fachrizi_kiki 0:e14308f43fdf 80 //pwm saturasi start
fachrizi_kiki 0:e14308f43fdf 81 if(pidPwm > 1){
fachrizi_kiki 0:e14308f43fdf 82 pwmLebih = 1;
fachrizi_kiki 0:e14308f43fdf 83 }else if(pidPwm < 1 && pidPwm > 0){
fachrizi_kiki 0:e14308f43fdf 84 pwmLebih = fabs(pidPwm);
fachrizi_kiki 0:e14308f43fdf 85 }else if(pidPwm < -1){
fachrizi_kiki 0:e14308f43fdf 86 pwmLebih = -1;
fachrizi_kiki 0:e14308f43fdf 87 }else if(pidPwm > -1 && pidPwm < 0){
fachrizi_kiki 0:e14308f43fdf 88 pwmLebih = fabs(pidPwm);
fachrizi_kiki 0:e14308f43fdf 89 }
fachrizi_kiki 0:e14308f43fdf 90 //pwm saturasi end
fachrizi_kiki 0:e14308f43fdf 91
fachrizi_kiki 0:e14308f43fdf 92 //motor start
fachrizi_kiki 0:e14308f43fdf 93 pwm_ = fabs(pwmLebih1);
fachrizi_kiki 0:e14308f43fdf 94 //motor end
fachrizi_kiki 0:e14308f43fdf 95
fachrizi_kiki 0:e14308f43fdf 96 lastPid = pidPwm;
fachrizi_kiki 0:e14308f43fdf 97 pc.printf("%f\n", pwmLebih);
fachrizi_kiki 0:e14308f43fdf 98 }
fachrizi_kiki 0:e14308f43fdf 99