library to use pid easier
PID_lib.cpp@9:2304689cab4a, 21 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?
User | Revision | Line number | New 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 |