fachrizi kiki / PID_lib

library to use pid easier

PID_lib.cpp@9:2304689cab4a, 17 months ago (annotated)

Committer:
fachrizi_kiki
Date:
Sun Jan 01 08:39:23 2023 +0000
Revision:
9:2304689cab4a
Parent:
8:07de6805e193 
update filter 15 hz dan 20 hz;

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