fachrizi kiki / PID_lib

library to use pid easier

PID_lib.cpp@4:344e46625032, 18 months ago (annotated)

Committer:
fachrizi_kiki
Date:
Thu Dec 15 00:58:59 2022 +0000
Revision:
4:344e46625032
Parent:
3:80e7ed9fdb02
Child:
5:ae183f132d39
update PID_lib

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 4:344e46625032 5 // ppr = 540.0f;
fachrizi_kiki 4:344e46625032 6 ppr = 1000.0f;
fachrizi_kiki 3:80e7ed9fdb02 7
fachrizi_kiki 0:e14308f43fdf 8 dir1 = 0;
fachrizi_kiki 0:e14308f43fdf 9 dir2 = 0;
fachrizi_kiki 1:ebb9cfc0cff5 10 pwm = 0;
fachrizi_kiki 0:e14308f43fdf 11
fachrizi_kiki 0:e14308f43fdf 12 float ppr = 540.0f;
fachrizi_kiki 0:e14308f43fdf 13 float phi = 3.14285714;
fachrizi_kiki 0:e14308f43fdf 14
fachrizi_kiki 4:344e46625032 15
fachrizi_kiki 3:80e7ed9fdb02 16 // t.start();
fachrizi_kiki 4:344e46625032 17 // tr.reset();
fachrizi_kiki 4:344e46625032 18 }
fachrizi_kiki 4:344e46625032 19
fachrizi_kiki 4:344e46625032 20 void PID_lib::manualPwm(int dir ,float speed_){
fachrizi_kiki 4:344e46625032 21 if(dir == 1){
fachrizi_kiki 4:344e46625032 22 dir1 = 0;
fachrizi_kiki 4:344e46625032 23 dir2 = 1;
fachrizi_kiki 4:344e46625032 24 pwm = speed_;
fachrizi_kiki 4:344e46625032 25 }else if(dir == 0){
fachrizi_kiki 4:344e46625032 26 dir1 = 1;
fachrizi_kiki 4:344e46625032 27 dir2 = 0;
fachrizi_kiki 4:344e46625032 28 pwm = speed_;
fachrizi_kiki 4:344e46625032 29 }
fachrizi_kiki 3:80e7ed9fdb02 30 }
fachrizi_kiki 3:80e7ed9fdb02 31
fachrizi_kiki 3:80e7ed9fdb02 32 void PID_lib::stop(){
fachrizi_kiki 3:80e7ed9fdb02 33 rpm = 0;
fachrizi_kiki 3:80e7ed9fdb02 34 rpmn1 = 0;
fachrizi_kiki 3:80e7ed9fdb02 35 rpmFilt = 0;
fachrizi_kiki 3:80e7ed9fdb02 36 rpmFiltn1 = 0;
fachrizi_kiki 3:80e7ed9fdb02 37 lastPid = 0;
fachrizi_kiki 3:80e7ed9fdb02 38 e = 0;
fachrizi_kiki 3:80e7ed9fdb02 39 eI = 0;
fachrizi_kiki 3:80e7ed9fdb02 40 eD = 0;
fachrizi_kiki 3:80e7ed9fdb02 41 pidPwm = 0;
fachrizi_kiki 4:344e46625032 42 pwm = 0;
fachrizi_kiki 4:344e46625032 43 dir1 = 0;
fachrizi_kiki 4:344e46625032 44 dir2 = 0;
fachrizi_kiki 3:80e7ed9fdb02 45 }
fachrizi_kiki 3:80e7ed9fdb02 46
fachrizi_kiki 3:80e7ed9fdb02 47 // kp ki kd untuk sp 50 tuning manual lagi stelah tuning matlab
fachrizi_kiki 3:80e7ed9fdb02 48 void PID_lib::pwm_read(float target_, float kp_,float ki_,float kd_, float freq, float t_){
fachrizi_kiki 3:80e7ed9fdb02 49 tim = t_;
fachrizi_kiki 3:80e7ed9fdb02 50 dt = tim - lastime;
fachrizi_kiki 3:80e7ed9fdb02 51
fachrizi_kiki 3:80e7ed9fdb02 52 rpm = freq/ppr*60;
fachrizi_kiki 3:80e7ed9fdb02 53
fachrizi_kiki 3:80e7ed9fdb02 54 rpmFilt = 0.03046875*rpm + 0.03046875*rpmn1 + 0.93906251*rpmFiltn1;//10hz filter
fachrizi_kiki 3:80e7ed9fdb02 55
fachrizi_kiki 3:80e7ed9fdb02 56 //error computing start
fachrizi_kiki 3:80e7ed9fdb02 57 e = target_ - rpmFilt;
fachrizi_kiki 3:80e7ed9fdb02 58 eI += e;
fachrizi_kiki 3:80e7ed9fdb02 59 eD = e - laste;
fachrizi_kiki 3:80e7ed9fdb02 60 //error computing end
fachrizi_kiki 3:80e7ed9fdb02 61
fachrizi_kiki 3:80e7ed9fdb02 62 //storing error value start
fachrizi_kiki 3:80e7ed9fdb02 63 hP = e*kp_;
fachrizi_kiki 3:80e7ed9fdb02 64 hI = eI*ki_*dt;
fachrizi_kiki 3:80e7ed9fdb02 65 hD = eD*kd_/dt;
fachrizi_kiki 3:80e7ed9fdb02 66 //storing error value end
fachrizi_kiki 3:80e7ed9fdb02 67 //saturasu ki start
fachrizi_kiki 3:80e7ed9fdb02 68 if(hI < 0.5 && hI > 0){
fachrizi_kiki 3:80e7ed9fdb02 69 setI = hI;
fachrizi_kiki 3:80e7ed9fdb02 70 }else if(hI > 0.5){
fachrizi_kiki 3:80e7ed9fdb02 71 setI = 0.5;
fachrizi_kiki 3:80e7ed9fdb02 72 }
fachrizi_kiki 3:80e7ed9fdb02 73 else if(hI < 0 && hI > -0.5){
fachrizi_kiki 3:80e7ed9fdb02 74 setI = hI;
fachrizi_kiki 3:80e7ed9fdb02 75 }else if(hI < -0.5){
fachrizi_kiki 3:80e7ed9fdb02 76 setI = -0.5;
fachrizi_kiki 3:80e7ed9fdb02 77 }
fachrizi_kiki 3:80e7ed9fdb02 78 //saturasi ki end
fachrizi_kiki 3:80e7ed9fdb02 79 pidPwm = hP+setI+hD;//pwm pid
fachrizi_kiki 3:80e7ed9fdb02 80 lastime = tim;//update timer
fachrizi_kiki 3:80e7ed9fdb02 81 laste = e;//update error
fachrizi_kiki 3:80e7ed9fdb02 82
fachrizi_kiki 3:80e7ed9fdb02 83 rpmFiltn1 = rpmFilt;rpmn1 = rpm;//update filter
fachrizi_kiki 3:80e7ed9fdb02 84 lastPid = pidPwm;
fachrizi_kiki 3:80e7ed9fdb02 85 // printf("%f;%f;%f\n",e,eI,eD);
fachrizi_kiki 3:80e7ed9fdb02 86 printf("%f;%f;%f\n", hP,setI,hD);
fachrizi_kiki 3:80e7ed9fdb02 87
fachrizi_kiki 0:e14308f43fdf 88 }
fachrizi_kiki 0:e14308f43fdf 89
fachrizi_kiki 0:e14308f43fdf 90
fachrizi_kiki 0:e14308f43fdf 91
fachrizi_kiki 3:80e7ed9fdb02 92 void PID_lib::pid_pwm(float target_, float kp_,float ki_,float kd_, float freq, float t_){
fachrizi_kiki 3:80e7ed9fdb02 93 tim = t_;
fachrizi_kiki 0:e14308f43fdf 94 dt = tim - lastime;
fachrizi_kiki 3:80e7ed9fdb02 95
fachrizi_kiki 3:80e7ed9fdb02 96 rpm = freq/ppr*60;
fachrizi_kiki 0:e14308f43fdf 97
fachrizi_kiki 0:e14308f43fdf 98 rpmFilt = 0.03046875*rpm + 0.03046875*rpmn1 + 0.93906251*rpmFiltn1;//10hz filter
fachrizi_kiki 0:e14308f43fdf 99
fachrizi_kiki 0:e14308f43fdf 100 //error computing start
fachrizi_kiki 3:80e7ed9fdb02 101 e = target_ - rpmFilt;
fachrizi_kiki 0:e14308f43fdf 102 eI += e;
fachrizi_kiki 0:e14308f43fdf 103 eD = e - laste;
fachrizi_kiki 0:e14308f43fdf 104 //error computing end
fachrizi_kiki 0:e14308f43fdf 105
fachrizi_kiki 0:e14308f43fdf 106 //storing error value start
fachrizi_kiki 0:e14308f43fdf 107 hP = e*kp_;
fachrizi_kiki 0:e14308f43fdf 108 hI = eI*ki_*dt;
fachrizi_kiki 0:e14308f43fdf 109 hD = eD*kd_/dt;
fachrizi_kiki 0:e14308f43fdf 110 //storing error value end
fachrizi_kiki 0:e14308f43fdf 111
fachrizi_kiki 0:e14308f43fdf 112 //saturasu ki start
fachrizi_kiki 0:e14308f43fdf 113 if(hI < 0.5 && hI > 0){
fachrizi_kiki 0:e14308f43fdf 114 setI = hI;
fachrizi_kiki 0:e14308f43fdf 115 }else if(hI > 0.5){
fachrizi_kiki 0:e14308f43fdf 116 setI = 0.5;
fachrizi_kiki 0:e14308f43fdf 117 }
fachrizi_kiki 0:e14308f43fdf 118 else if(hI < 0 && hI > -0.5){
fachrizi_kiki 0:e14308f43fdf 119 setI = hI;
fachrizi_kiki 0:e14308f43fdf 120 }else if(hI < -0.5){
fachrizi_kiki 0:e14308f43fdf 121 setI = -0.5;
fachrizi_kiki 0:e14308f43fdf 122 }
fachrizi_kiki 0:e14308f43fdf 123 //saturasi ki end
fachrizi_kiki 0:e14308f43fdf 124
fachrizi_kiki 0:e14308f43fdf 125 pidPwm = hP+setI+hD;//pwm pid
fachrizi_kiki 0:e14308f43fdf 126 lastime = tim;//update timer
fachrizi_kiki 0:e14308f43fdf 127 laste = e;//update error
fachrizi_kiki 0:e14308f43fdf 128
fachrizi_kiki 0:e14308f43fdf 129 rpmFiltn1 = rpmFilt;rpmn1 = rpm;//update filter
fachrizi_kiki 0:e14308f43fdf 130 //motor direksi start
fachrizi_kiki 0:e14308f43fdf 131 if(pidPwm < 0){dir1 = 1; dir2 = 0;}
fachrizi_kiki 0:e14308f43fdf 132 else{dir1 = 0; dir2 = 1;}
fachrizi_kiki 0:e14308f43fdf 133 //motor direksi end
fachrizi_kiki 0:e14308f43fdf 134
fachrizi_kiki 0:e14308f43fdf 135 //pwm saturasi start
fachrizi_kiki 0:e14308f43fdf 136 if(pidPwm > 1){
fachrizi_kiki 0:e14308f43fdf 137 pwmLebih = 1;
fachrizi_kiki 0:e14308f43fdf 138 }else if(pidPwm < 1 && pidPwm > 0){
fachrizi_kiki 0:e14308f43fdf 139 pwmLebih = fabs(pidPwm);
fachrizi_kiki 0:e14308f43fdf 140 }else if(pidPwm < -1){
fachrizi_kiki 0:e14308f43fdf 141 pwmLebih = -1;
fachrizi_kiki 0:e14308f43fdf 142 }else if(pidPwm > -1 && pidPwm < 0){
fachrizi_kiki 0:e14308f43fdf 143 pwmLebih = fabs(pidPwm);
fachrizi_kiki 0:e14308f43fdf 144 }
fachrizi_kiki 0:e14308f43fdf 145 //pwm saturasi end
fachrizi_kiki 0:e14308f43fdf 146
fachrizi_kiki 0:e14308f43fdf 147 //motor start
fachrizi_kiki 2:77ef3d60d8d9 148 pwm = fabs(pwmLebih);
fachrizi_kiki 0:e14308f43fdf 149 //motor end
fachrizi_kiki 0:e14308f43fdf 150
fachrizi_kiki 0:e14308f43fdf 151 lastPid = pidPwm;
fachrizi_kiki 3:80e7ed9fdb02 152 // printf("%f\n", rpmFilt);
fachrizi_kiki 3:80e7ed9fdb02 153 // printf("%f;%f;%f\n",e,eI,eD);
fachrizi_kiki 3:80e7ed9fdb02 154 printf("%f;%f\n", rpm, rpmFilt);
fachrizi_kiki 3:80e7ed9fdb02 155 // printf("%f;%f;%f\n",hP,setI,hD);
fachrizi_kiki 3:80e7ed9fdb02 156 // pc.printf("%f;%1lu\n",pidPwm,t1.read_high_resolution_us());
fachrizi_kiki 0:e14308f43fdf 157 }
fachrizi_kiki 0:e14308f43fdf 158
fachrizi_kiki 4:344e46625032 159 void PID_lib::record_data(int dir_,float speed_, float freq_rec, float t_r){
fachrizi_kiki 4:344e46625032 160 rpm_rec = freq_rec/ppr*60;
fachrizi_kiki 4:344e46625032 161 if(dir_ == 1){
fachrizi_kiki 4:344e46625032 162 pwm = speed_;
fachrizi_kiki 4:344e46625032 163 dir1 = 1;
fachrizi_kiki 4:344e46625032 164 dir2 = 0;
fachrizi_kiki 4:344e46625032 165 }else if(dir_ == 0){
fachrizi_kiki 4:344e46625032 166 pwm = speed_;
fachrizi_kiki 4:344e46625032 167 dir1 = 1;
fachrizi_kiki 4:344e46625032 168 dir2 = 0;
fachrizi_kiki 4:344e46625032 169 }
fachrizi_kiki 4:344e46625032 170 printf("%f;%.2f;%llu\n",speed_,rpm_rec,t_r);
fachrizi_kiki 4:344e46625032 171 }
fachrizi_kiki 4:344e46625032 172