2022_gorobo_Ateam / gourobo2022_undercarriage_new

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

Dependencies:   SBDBT arrc_mbed BNO055

Diff: PIDco.cpp

Revision:
10:ad8fced7d6b6
Parent:
9:569c0f55dd9b
Child:
11:264f992664b0
--- a/PIDco.cpp	Thu Feb 03 13:07:37 2022 +0000
+++ b/PIDco.cpp	Fri Mar 11 04:24:27 2022 +0000
@@ -33,7 +33,7 @@
     Kd = d_gain;
 }
 
-void PIDco::wheel_ctl(PinName PIN_A,PinName PIN_B){
+void PIDco::wheel_ctl(PinName PIN_A,PinName PIN_B,double Regulation){
     PwmOut v1p(PIN_A);
     PwmOut v1m(PIN_B);
     
@@ -46,6 +46,7 @@
     cal_Ival();
     cal_Dval();
     cal_pwm();
+    Pwm = Pwm * Regulation;
     
         if(Pwm > 0){
             v1p = Pwm > 0.5 ? 0.5 : Pwm;
@@ -60,19 +61,20 @@
     renew_Error();
 }
     
-void PIDco::angle_ctl(double now_angle,double TARGET,double p_gain,double i_gain,double d_gain){
-    pulse_a = now_angle;
+void PIDco::pid_ctl(double now_angle,double TARGET,double p_gain,double i_gain,double d_gain){
     Target = TARGET;
     Kp = p_gain;
     Ki = i_gain;
     Kd = d_gain;
-    cal_spd();
-    cal_Error();
+    
+    Error_a = Target - now_angle;
     cal_Pval();
     cal_Ival();
     cal_Dval();
-    cal_pwm();
+    Pwm = (Pval + Ival + Dval);
     output = Pwm;
+    //printf("%lf\n",Error_a);
+    renew_Error();
 }
 
 void PIDco::cal_Error(){ Error_a = Target - spd; }