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