library to use pid easier
PID_lib.cpp
- Committer:
- fachrizi_kiki
- Date:
- 23 months ago
- Revision:
- 0:e14308f43fdf
- Child:
- 1:ebb9cfc0cff5
File content as of revision 0:e14308f43fdf:
#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); }