library to use pid easier
PID_lib.cpp@4:344e46625032, 21 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?
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 | 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 |