library to use pid easier
Diff: PID_lib.cpp
- Revision:
- 0:e14308f43fdf
- Child:
- 1:ebb9cfc0cff5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID_lib.cpp Sun Oct 16 09:27:54 2022 +0000 @@ -0,0 +1,99 @@ +#include "PID_lib.h" +#include "mbed.h" + +PID_lib::PID_lib(PinName direksi1, PinName direksi2, PinName pulseWidth, float rpm) : dir1(direksi1), dir2(direksi2), pwm_(pulseWidth), rpm_(rpm){ + rpm = rpm_; + dir1 = 0; + dir2 = 0; + pwm_ = 0; + + float kpA = 0.007770; + float kiA = 0.003297; + float kdA = 0.000015; + + float kpB = 0.03134; + float kiB = 0.003605; + float kdB = 0.001002; + + float kpC = 0.02040; + float kiC = 0.003292; + float kdC = 0.000015; + + float kpD = 0.02031; + float kiD = 0.002760; + float kdD = 3.943e-04; + + float ppr = 540.0f; + float phi = 3.14285714; + + t.start(); + t.reset(); +} + + + +// kp ki kd untuk sp 50 tuning manual lagi stelah tuning matlab + + + +void pid_pwm(float target, float kp_,float ki_,float kd_){ + tim = t; + dt = tim - lastime; + + rpmFilt = 0.03046875*rpm + 0.03046875*rpmn1 + 0.93906251*rpmFiltn1;//10hz filter + + //error computing start + e = sp - rpmFilt; + eI += e; + eD = e - laste; + //error computing end + + //storing error value start + hP = e*kp_; + hI = eI*ki_*dt; + hD = eD*kd_/dt; + //storing error value end + + //saturasu ki start + if(hI < 0.5 && hI > 0){ + setI = hI; + }else if(hI > 0.5){ + setI = 0.5; + } + else if(hI < 0 && hI > -0.5){ + setI = hI; + }else if(hI < -0.5){ + setI = -0.5; + } + //saturasi ki end + + pidPwm = hP+setI+hD;//pwm pid + lastime = tim;//update timer + laste = e;//update error + + rpmFiltn1 = rpmFilt;rpmn1 = rpm;//update filter + //motor direksi start + if(pidPwm < 0){dir1 = 1; dir2 = 0;} + else{dir1 = 0; dir2 = 1;} + //motor direksi end + + //pwm saturasi start + if(pidPwm > 1){ + pwmLebih = 1; + }else if(pidPwm < 1 && pidPwm > 0){ + pwmLebih = fabs(pidPwm); + }else if(pidPwm < -1){ + pwmLebih = -1; + }else if(pidPwm > -1 && pidPwm < 0){ + pwmLebih = fabs(pidPwm); + } + //pwm saturasi end + + //motor start + pwm_ = fabs(pwmLebih1); + //motor end + + lastPid = pidPwm; + pc.printf("%f\n", pwmLebih); +} +