2022_gorobo_Ateam / gourobo2022_undercarriage_new

2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。

Dependencies:   SBDBT arrc_mbed BNO055

Diff: PIDco.cpp

Revision:
7:a0375e6dc8ca
Parent:
6:e089fda81b74
Child:
9:569c0f55dd9b
--- a/PIDco.cpp	Mon Jan 24 09:21:43 2022 +0000
+++ b/PIDco.cpp	Thu Jan 27 09:21:50 2022 +0000
@@ -1,4 +1,7 @@
 #include "PIDco.hpp"
+#include "mbed.h"
+
+Serial pc(USBTX,USBRX);
 
 PIDco::PIDco(){
     spd =  0.0;
@@ -44,7 +47,7 @@
     cal_Dval();
     cal_pwm();
     
-    if(Pwm > 0){
+        if(Pwm > 0){
             v1p = Pwm > 0.5 ? 0.5 : Pwm;
             v1m = 0;
         }else if(Pwm < 0){
@@ -57,6 +60,20 @@
     renew_Error();
 }
     
+void PIDco::angle_ctl(double PULSE,double TARGET,double p_gain,double i_gain,double d_gain){
+    pulse_a = PULSE;
+    Target = TARGET;
+    Kp = p_gain;
+    Ki = i_gain;
+    Kd = d_gain;
+    cal_spd();
+    cal_Error();
+    cal_Pval();
+    cal_Ival();
+    cal_Dval();
+    cal_pwm();
+    output = Pwm;
+}
 
 void PIDco::cal_Error(){ Error_a = Target - spd; }
 
@@ -64,14 +81,14 @@
 
 void PIDco::cal_Dval(){ Dval = Kd * (Error_a - Error_b) / dt; }
 
-void PIDco::cal_pwm(){ Pwm += (Pval + Ival + Dval); }
+void PIDco::cal_pwm(){ 
+    Pwm += (Pval + Ival + Dval); 
+    if(Pwm <= 0.0001 && Pwm > -0.0001){
+        Pwm = 0.0;
+    }
+    //printf("%lf\n",Pwm);
+}
 
 void PIDco::renew_Error(){ Error_b = Error_a; }
 
 double PIDco::obt_spd(){ return spd; }
-
-
-
-
-    
-    
\ No newline at end of file