library to use pid easier
Diff: PID_lib.cpp
- Revision:
- 3:80e7ed9fdb02
- Parent:
- 2:77ef3d60d8d9
- Child:
- 4:344e46625032
--- a/PID_lib.cpp Sun Oct 16 11:14:26 2022 +0000 +++ b/PID_lib.cpp Thu Oct 20 07:36:28 2022 +0000 @@ -2,7 +2,8 @@ PID_lib::PID_lib(PinName direksi1, PinName direksi2, PinName pulseWidth) : dir1(direksi1), dir2(direksi2), pwm(pulseWidth){ - rpm = rpm; + ppr = 540.0f; + dir1 = 0; dir2 = 0; pwm = 0; @@ -10,24 +11,77 @@ float ppr = 540.0f; float phi = 3.14285714; - t.start(); - t.reset(); +// t.start(); +// t.reset(); +} + +void PID_lib::stop(){ + rpm = 0; + rpmn1 = 0; + rpmFilt = 0; + rpmFiltn1 = 0; + lastPid = 0; + e = 0; + eI = 0; + eD = 0; + pidPwm = 0; +} + +// kp ki kd untuk sp 50 tuning manual lagi stelah tuning matlab +void PID_lib::pwm_read(float target_, float kp_,float ki_,float kd_, float freq, float t_){ + tim = t_; + dt = tim - lastime; + + rpm = freq/ppr*60; + + rpmFilt = 0.03046875*rpm + 0.03046875*rpmn1 + 0.93906251*rpmFiltn1;//10hz filter + + //error computing start + e = target_ - 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 + lastPid = pidPwm; +// printf("%f;%f;%f\n",e,eI,eD); + printf("%f;%f;%f\n", hP,setI,hD); + } -// kp ki kd untuk sp 50 tuning manual lagi stelah tuning matlab - - - -void PID_lib::pid_pwm(float target, float kp_,float ki_,float kd_, float rpm){ - tim = t; +void PID_lib::pid_pwm(float target_, float kp_,float ki_,float kd_, float freq, float t_){ + tim = t_; dt = tim - lastime; + + rpm = freq/ppr*60; rpmFilt = 0.03046875*rpm + 0.03046875*rpmn1 + 0.93906251*rpmFiltn1;//10hz filter //error computing start - e = target - rpmFilt; + e = target_ - rpmFilt; eI += e; eD = e - laste; //error computing end @@ -78,6 +132,10 @@ //motor end lastPid = pidPwm; - printf("%f\n", pwmLebih); +// printf("%f\n", rpmFilt); +// printf("%f;%f;%f\n",e,eI,eD); + printf("%f;%f\n", rpm, rpmFilt); +// printf("%f;%f;%f\n",hP,setI,hD); +// pc.printf("%f;%1lu\n",pidPwm,t1.read_high_resolution_us()); }