Diff: PIDco.cpp

Revision:

0:a1238c4cd105

Child:

1:ea880e226e5a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDco.cpp	Wed Jan 19 12:43:26 2022 +0000
@@ -0,0 +1,75 @@
+#include "PIDco.hpp"
+
+PIDco::PIDco(){
+    spd = 0.0;
+    Pwm = 0.0;
+    dt = 0.05;
+    Integral = 0.0;
+    Error_b = 0.0;
+    Error_a = 0.0;
+    diff = 0.0;
+    pulse_b = 0.0;
+    }
+    
+void PIDco::cal_spd(){
+    diff = pulse_a - pulse_b;
+    spd = co * diff / dt;
+    pulse_b = pulse_a;
+}
+
+void PIDco::cal_Ival(){
+    Integral += (Error_a + Error_b) / 2.0 * dt;
+    Ival = Ki * Integral;
+}
+
+void PIDco::pass_val(double PULSE,double TARGET){
+    pulse_a = PULSE;
+    Target = TARGET;
+}
+
+void PIDco::wheel_ctl(PinName PIN_A,PinName PIN_B){
+    PwmOut v1p(PIN_A);
+    PwmOut v1m(PIN_B);
+    
+    v1p.period_us(2048);
+    v1m.period_us(2048);
+    
+    cal_spd();
+    cal_Error();
+    cal_Pval();
+    cal_Ival();
+    cal_Dval();
+    cal_pwm();
+    
+    if(Pwm > 0){
+            v1p = Pwm > 0.5 ? 0.5 : Pwm;
+            v1m = 0;
+        }else if(Pwm < 0){
+            v1p = 0;
+            v1m = -Pwm > 0.5 ? 0.5 : -Pwm;
+        }else{
+            v1p = 0;
+            v1m = 0;
+        }
+    
+    renew_Error();
+}
+    
+
+void PIDco::cal_Error(){ Error_a = Target - spd; }
+
+void PIDco::cal_Pval(){ Pval = Kp * Error_a; }
+
+void PIDco::cal_Dval(){ Dval = Kd * (Error_a - Error_b) / dt; }
+
+void PIDco::cal_pwm(){ Pwm += (Pval + Ival + Dval); }
+
+void PIDco::renew_Error(){ Error_b = Error_a; }
+
+double PIDco::obt_spd(){ return spd; }
+
+
+
+
+    
+    
\ No newline at end of file