library to use pid easier
PID_lib.cpp
- Committer:
- fachrizi_kiki
- Date:
- 2022-10-16
- Revision:
- 2:77ef3d60d8d9
- Parent:
- 1:ebb9cfc0cff5
- Child:
- 3:80e7ed9fdb02
File content as of revision 2:77ef3d60d8d9:
#include "PID_lib.h" PID_lib::PID_lib(PinName direksi1, PinName direksi2, PinName pulseWidth) : dir1(direksi1), dir2(direksi2), pwm(pulseWidth){ rpm = rpm; dir1 = 0; dir2 = 0; pwm = 0; 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_lib::pid_pwm(float target, float kp_,float ki_,float kd_, float rpm){ tim = t; dt = tim - lastime; 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 //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(pwmLebih); //motor end lastPid = pidPwm; printf("%f\n", pwmLebih); }