![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
青木制作機体プログラム
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
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; //曲率演算にて求めた2走目以降の仮想走行速度 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倍以上かつ右が左の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(); //2走目の場合はブレーキングシステムを使用する。 + + //センサ取得値の重ね合わせ(端のセンサほどモータ制御量を大きくする) 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(); //2走目の場合はブレーキングシステムを使用する。 + //割込み終了時の各種パラメータリセット処理 //エンコーダ関連情報のリセット encoder_a.Set(0);//エンコーダクラスのパルス数情報のリセット