library to use pid easier
PID_lib.cpp@1:ebb9cfc0cff5, 23 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?
User | Revision | Line number | New 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 |