fachrizi kiki / PID_lib

library to use pid easier

PID_lib.cpp@1:ebb9cfc0cff5, 20 months ago (annotated)

Committer:
fachrizi_kiki
Date:
Sun Oct 16 10:07:21 2022 +0000
Revision:
1:ebb9cfc0cff5
Parent:
0:e14308f43fdf
Child:
2:77ef3d60d8d9
update

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 1:ebb9cfc0cff5 4 PID_lib::PID_lib(PinName direksi1, PinName direksi2, PinName pulseWidth, float rpm) : dir1(direksi1), dir2(direksi2), pwm(pulseWidth), rpm(rpm){
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 1:ebb9cfc0cff5 10 // float kpA = 0.007770;
fachrizi_kiki 1:ebb9cfc0cff5 11 // float kiA = 0.003297;
fachrizi_kiki 1:ebb9cfc0cff5 12 // float kdA = 0.000015;
fachrizi_kiki 1:ebb9cfc0cff5 13 //
fachrizi_kiki 1:ebb9cfc0cff5 14 // float kpB = 0.03134;
fachrizi_kiki 1:ebb9cfc0cff5 15 // float kiB = 0.003605;
fachrizi_kiki 1:ebb9cfc0cff5 16 // float kdB = 0.001002;
fachrizi_kiki 1:ebb9cfc0cff5 17 //
fachrizi_kiki 1:ebb9cfc0cff5 18 // float kpC = 0.02040;
fachrizi_kiki 1:ebb9cfc0cff5 19 // float kiC = 0.003292;
fachrizi_kiki 1:ebb9cfc0cff5 20 // float kdC = 0.000015;
fachrizi_kiki 1:ebb9cfc0cff5 21 //
fachrizi_kiki 1:ebb9cfc0cff5 22 // float kpD = 0.02031;
fachrizi_kiki 1:ebb9cfc0cff5 23 // float kiD = 0.002760;
fachrizi_kiki 1:ebb9cfc0cff5 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 1:ebb9cfc0cff5 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