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);
+}
+