Diff: main.cpp

Revision:

32:4deb27262901

Parent:

31:fe9ae7992246

--- a/main.cpp	Tue Nov 26 02:57:02 2019 +0000
+++ b/main.cpp	Wed Nov 27 08:40:41 2019 +0000
@@ -46,36 +46,36 @@
 double      Sensor_PD=0.0f;         //ラインセンサP,D成分の合計
 char        Gray_Str[5];            //LCD閾値表示用文字列
 float       Gray=DEFAULT_GRAY;      //白黒のしきい値設定
-long long int    Enc_Count_A=0,Enc_Count_B=0;    //エンコーダパルス数。現在速度推定に使用する。
-long long int    Enc_A_Rotate=0,Enc_B_Rotate=0;  //エンコーダパルス数。停止時の機体速度調整に使用。
-long long int    Stop_Distance=STOP_DISTANCE;    //ストップ時の徐行→停止に使う
-long long int    Next_Marker_Distance=200000;         //加減速走行時のマーカ間の距離を格納
-long long int    Recent_Distance=0;              //加減速走行時の次のマーカまでの距離を格納
+long int         Enc_Count_A=0,Enc_Count_B=0;    //エンコーダパルス数。現在速度推定に使用する。
+long int         Enc_A_Rotate=0,Enc_B_Rotate=0;  //エンコーダパルス数。停止時の機体速度調整に使用。
+long int       Stop_Distance=STOP_DISTANCE;    //ストップ時の徐行→停止に使う
+long int       Next_Marker_Distance=200000;         //加減速走行時のマーカ間の距離を格納
+long int       Recent_Distance=0;              //加減速走行時の次のマーカまでの距離を格納
         /////↓エンコーダパルス数。コース記憶時のエンコーダパルスの蓄積に使用する。
-long long int    Memory_Enc_Count_A=0,Memory_Enc_Count_B=0;   
+long int         Memory_Enc_Count_A=0,Memory_Enc_Count_B=0;   
 char        MemoryA_Str[5];                 //LCD表示用。左モータの走行距離格納
 char        MemoryB_Str[5];                 //LCD表示用。右モータの走行距離格納
-long long int    Distance_A=0,Distance_B=0;      //タイヤ移動距離を格納[mm]。
+long int       Distance_A=0,Distance_B=0;      //タイヤ移動距離を格納[mm]。
                                             //機体の現在速度の推定に使用する。タイマ割込みごとにリセットされる。
             ////↓左右タイヤの走行距離格納。1走目のコース記憶処理に使用する。マーカ通過ごとにリセット。                                
-long long int    Distance_memory_A=0, Distance_memory_B=0;
+long int       Distance_memory_A=0, Distance_memory_B=0;
 
-long int    S_Kp=S_KP_LOW;////センサP成分
-long int    S_Kd=S_KD_LOW;////センサD成分
+float       S_Kp=S_KP_LOW;////センサP成分
+float       S_Kd=S_KD_LOW;////センサD成分
 
-long long int Marker_Run_Distance=0; 
-long int Speed_A=0,  Speed_B=0;             //機体の現在速度を格納
-long int Low_Speed      = LOW_SPEED;        //2走目以降の低速
-long int Medium_Speed   = MEDIUM_SPEED;     //2走目以降の中速
-long int High_Speed     = HIGH_SPEED;       //2走目以降の高速
-char     Speed_Str[5];                      //LCD速度表示用文字列
-long int Target_Speed_A=0,Target_Speed_B=0; //目標速度
-long int Motor_A_Diff[2]={0,0};             //過去の速度偏差と現在の速度偏差を格納
-long int Motor_B_Diff[2]={0,0};             //
-float Motor_A_P,Motor_B_P;                  //モータ速度制御P成分
-float Motor_A_D,Motor_B_D;                  //モータ速度制御D成分
-float Motor_A_PD,Motor_B_PD;                //モータ速度制御PD合成
-float Motor_A_Pwm,Motor_B_Pwm;              //モータへの出力
+float       Marker_Run_Distance=0; 
+int         Speed_A=0,  Speed_B=0;             //機体の現在速度を格納
+int         Low_Speed      = LOW_SPEED;        //2走目以降の低速
+int         Medium_Speed   = MEDIUM_SPEED;     //2走目以降の中速
+int         High_Speed     = HIGH_SPEED;       //2走目以降の高速
+char        Speed_Str[5];                      //LCD速度表示用文字列
+int         Target_Speed_A=0,Target_Speed_B=0; //目標速度
+int         Motor_A_Diff[2]={0,0};             //過去の速度偏差と現在の速度偏差を格納
+int         Motor_B_Diff[2]={0,0};             //
+float       Motor_A_P,Motor_B_P;                  //モータ速度制御P成分
+float       Motor_A_D,Motor_B_D;                  //モータ速度制御D成分
+float       Motor_A_PD,Motor_B_PD;                //モータ速度制御PD合成
+float       Motor_A_Pwm,Motor_B_Pwm;              //モータへの出力
 unsigned char Sensor_Digital    =0x00;
 unsigned char Old_Sensor_Digital=0x00;
 int   Sensor_Cnt=0;
@@ -90,7 +90,7 @@
 
 int Imaginary_Speed=0;                 //曲率演算にて求めた２走目以降の仮想走行速度
 int Next_Imaginary_Speed=0;
-long long int course_data[100][3];                  //コース情報の記憶用配列マーカ数は行数分まで対応し、
+long int       course_data[100][3];                  //コース情報の記憶用配列マーカ数は行数分まで対応し、
                                             //列情報は左タイヤ走行距離、右タイヤ走行距離、演算で求めた2走目走行速度を格納する。
 int Row=0;                                  //行情報を格納。マーカ通過ごとに加算し、走行終了後リセットされる。
 int Marker_Cnt=0;
@@ -192,13 +192,13 @@
     //左右タイヤの走行距離の差分を用いた曲率演算
              /////左が右の一倍以上かつ左が右の1.2倍以下もしくは右は左の１倍以上かつ右が左の1.2倍以下の時
     if((((course_data[Row][0])>=(course_data[Row][1]*1.0f))&&((course_data[Row][0])<(course_data[Row][1]*HIGH_SPEED_SECTION)))
-      || (((course_data[Row][0])>=(course_data[Row][1]*1.0f))&&((course_data[Row][0])<(course_data[Row][1]*HIGH_SPEED_SECTION)))){
+      || (((course_data[Row][1])>=(course_data[Row][0]*1.0f))&&((course_data[Row][1])<(course_data[Row][0]*HIGH_SPEED_SECTION)))){
         Imaginary_Speed=High_Speed;//高速とする。    
     }else if((((course_data[Row][0])>=(course_data[Row][1]*HIGH_SPEED_SECTION))&&((course_data[Row][0])<(course_data[Row][1]*MEDIUM_SPEED_SECTION)))
-      || (((course_data[Row][0])>=(course_data[Row][1]*HIGH_SPEED_SECTION))&&((course_data[Row][0])<(course_data[Row][1]*MEDIUM_SPEED_SECTION)))){
+      || (((course_data[Row][1])>=(course_data[Row][0]*HIGH_SPEED_SECTION))&&((course_data[Row][1])<(course_data[Row][0]*MEDIUM_SPEED_SECTION)))){
         Imaginary_Speed=Medium_Speed;//中カーブは中速とする。
     }else if((((course_data[Row][0])>=(course_data[Row][1]*MEDIUM_SPEED_SECTION))&&((course_data[Row][0])<(course_data[Row][1]*LOW_SPEED_SECTION)))
-      || (((course_data[Row][0])>=(course_data[Row][1]*MEDIUM_SPEED_SECTION))&&((course_data[Row][0])<(course_data[Row][1]*LOW_SPEED_SECTION)))){
+      || (((course_data[Row][1])>=(course_data[Row][0]*MEDIUM_SPEED_SECTION))&&((course_data[Row][1])<(course_data[Row][0]*LOW_SPEED_SECTION)))){
         Imaginary_Speed=Low_Speed;//低速
     }
     else{
@@ -254,23 +254,26 @@
                     -(((Memory_Enc_Count_A*PULSE_TO_UM)+(Memory_Enc_Count_B*PULSE_TO_UM))/2);
 
     //現在速度がハイスピードで次のコースが急カーブであり、残り距離が---だったとき
-    if((course_data[Row][2]==High_Speed)&&(Next_Imaginary_Speed==Low_Speed)
-        &&(Recent_Distance<HL_BREAK_DISANCE)){
-            Target_Speed_A=Low_Speed;
-            Target_Speed_B=Low_Speed;
+    if((course_data[Row][2]==High_Speed)&&(Next_Imaginary_Speed<Medium_Speed)){           
+            if(Recent_Distance<(HL_BREAK_DISANCE)){
+                Target_Speed_A=Low_Speed;
+                Target_Speed_B=Low_Speed;
+            }                 
     }
     //現在速度がハイスピードで次のコースが中カーブであり、残り距離が---だったとき
-    else if((course_data[Row][2]==High_Speed)&&(Next_Imaginary_Speed==Medium_Speed)
-        &&(Recent_Distance<HM_BREAK_DISANCE)){
-            Target_Speed_A=Medium_Speed;
-            Target_Speed_B=Medium_Speed;
+    else if((course_data[Row][2]==High_Speed)&&(Next_Imaginary_Speed==Medium_Speed)){
+            if(Recent_Distance<(HM_BREAK_DISANCE)){
+                Target_Speed_A=Low_Speed;
+                Target_Speed_B=Low_Speed;
+            }
     }
     //現在速度が中間速度で次のコースが急カーブであり、残り距離が---だったとき
-    else if((course_data[Row][2]==High_Speed)&&(Next_Imaginary_Speed==Low_Speed)
-        &&(Recent_Distance<ML_BREAK_DISANCE)){
-            Target_Speed_A=Low_Speed;
-            Target_Speed_B=Low_Speed;
-    }    
+    else if((course_data[Row][2]==High_Speed)&&(Next_Imaginary_Speed<Medium_Speed)){
+            if(Recent_Distance<(ML_BREAK_DISANCE)){
+                Target_Speed_A=Low_Speed;
+                Target_Speed_B=Low_Speed;
+            } 
+    }          
 }
 
 /////LCD表示用関数。機体の現在状態を表示する。
@@ -362,7 +365,13 @@
         Marker_Run_Distance=0;                  //マーカ通過距離情報リセット
         
     }/////////////////各種マーカ判定処理の終了
+    //////コース記憶に使用するエンコーダパルス数の蓄積処理
+    Memory_Enc_Count_A+=Enc_Count_A;
+    Memory_Enc_Count_B+=Enc_Count_B;     
     
+    if(Sw_Cnt==2)second_breaking();     //２走目の場合はブレーキングシステムを使用する。   
+    
+        
     //センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする）
     All_Sensor_Data=-(S2_Data*S_K3+S3_Data*S_K2+S4_Data*S_K1)+(S5_Data*S_K1+S6_Data*S_K2+S7_Data*S_K3);
     Sensor_Diff[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避
@@ -441,12 +450,10 @@
     if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
         Marker_Run_Distance+=(Distance_A+Distance_B)/2;
     }
-    //////コース記憶に使用するエンコーダパルス数の蓄積処理
-    Memory_Enc_Count_A+=Enc_Count_A;
-    Memory_Enc_Count_B+=Enc_Count_B;                
+               
         
         
-    if(Sw_Cnt==2)second_breaking();     //２走目の場合はブレーキングシステムを使用する。           
+        
     //割込み終了時の各種パラメータリセット処理 
     //エンコーダ関連情報のリセット
     encoder_a.Set(0);//エンコーダクラスのパルス数情報のリセット