![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
加減速走行時の次マーカ検出前のブレーキングシステムを導入。各速度域におけるブレーキのタイミングとPDパラメータの調整を入念に行うこと。また、LCDの表示事項はストップゴールの表示は必要性を感じなかったため、走行交差点数のパラメータ表示に変更した。
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 28:00cb072ad251
- Parent:
- 27:9e234e087e70
- Child:
- 29:01b7f40ec029
--- a/main.cpp Mon Nov 25 03:17:25 2019 +0000 +++ b/main.cpp Mon Nov 25 05:37:20 2019 +0000 @@ -99,22 +99,22 @@ double Sensor_PD=0.0f; //ラインセンサP,D成分の合計 char Gray_Str[5]; //LCD閾値表示用文字列 float Gray=DEFAULT_GRAY; //白黒のしきい値設定 -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 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 Memory_Enc_Count_A=0,Memory_Enc_Count_B=0; char MemoryA_Str[5]; //LCD表示用。左モータの走行距離格納 char MemoryB_Str[5]; //LCD表示用。右モータの走行距離格納 -long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm]。 +long long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm]。 //機体の現在速度の推定に使用する。タイマ割込みごとにリセットされる。 ////↓左右タイヤの走行距離格納。1走目のコース記憶処理に使用する。マーカ通過ごとにリセット。 -long int Distance_memory_A=0, Distance_memory_B=0; +long long int Distance_memory_A=0, Distance_memory_B=0; long int S_Kp=S_KP_DEFAULT;////センサP成分 long int S_Kd=S_KD_DEFAULT;////センサD成分 -long int Marker_Run_Distance=0; +long long int Marker_Run_Distance=0; long int Speed_A=0, Speed_B=0; //機体の現在速度を格納 long int Default_Speed = DEFAULT_SPEED; //1走目の標準速度 long int Low_Speed = LOW_SPEED; //2走目以降の低速 @@ -228,17 +228,24 @@ course_data[Row][1]=(Memory_Enc_Count_B*PULSE_TO_UM);//右タイヤのマーカ間の走行距離を格納 //左右タイヤの走行距離の差分を用いた曲率演算 - if(((course_data[Row][0])<(course_data[Row][1]*1.2f)) || ((course_data[Row][1])<(course_data[Row][0]*1.2f))){//左右の回転差が20%以下の時 + /////左が右の一倍以上かつ左が右の1.2倍以下もしくは右は左の1倍以上かつ右が左の1.2倍以下の時 + if((((course_data[Row][0])>=(course_data[Row][1]*1.0f))&&((course_data[Row][0])<(course_data[Row][1]*1.2f))) + || (((course_data[Row][0])>=(course_data[Row][1]*1.0f))&&((course_data[Row][0])<(course_data[Row][1]*1.2f)))){ Imaginary_Speed=High_Speed;//高速とする。 - }else if((course_data[Row][0]<(course_data[Row][1]*1.5)) || (course_data[Row][1]<(course_data[Row][0]*1.5))){//左右の回転差が50%以下の時 + }else if((((course_data[Row][0])>=(course_data[Row][1]*1.2f))&&((course_data[Row][0])<(course_data[Row][1]*1.5f))) + || (((course_data[Row][0])>=(course_data[Row][1]*1.2f))&&((course_data[Row][0])<(course_data[Row][1]*1.5f)))){ Imaginary_Speed=Medium_Speed;//中カーブは中速とする。 - }else if((course_data[Row][0]<(course_data[Row][1]*2)) || (course_data[Row][1]<(course_data[Row][0]*2))){//左右の回転差が100%以下の時 + }else if((((course_data[Row][0])>=(course_data[Row][1]*1.5f))&&((course_data[Row][0])<(course_data[Row][1]*2.0f))) + || (((course_data[Row][0])>=(course_data[Row][1]*1.5f))&&((course_data[Row][0])<(course_data[Row][1]*2.0f)))){ Imaginary_Speed=Low_Speed;//低速 } else{ Imaginary_Speed=Low_Speed;//低速 } course_data[Row][2]=Imaginary_Speed; //仮想の演算速度を格納 + Memory_Enc_Count_A=0; //マーカ間の左タイヤエンコーダパルス数のクリア + Memory_Enc_Count_B=0; //マーカ間の右タイヤエンコーダパルス数のクリア + Row++; //次のコース情報へ } @@ -261,7 +268,8 @@ S_Kd=S_KD_DEFAULT_HIGH; } Target_Speed_A=course_data[Row][2];//記憶走行で演算した仮想速度を使う - Target_Speed_B=course_data[Row][2]; + Target_Speed_B=course_data[Row][2]; + Row++; //次のコース情報へ } @@ -312,11 +320,7 @@ Enc_Count_A=encoder_a.Get(); Enc_Count_B=-encoder_b.Get(); - if(!(Machine_Status&STOP)){//機体走行中のとき - //////コース記憶に使用するエンコーダパルス数の蓄積処理 - Memory_Enc_Count_A+=Enc_Count_A; - Memory_Enc_Count_B+=Enc_Count_B; - } + //各種マーカの検知 Old_Marker_Pass_Flag=Marker_Pass_Flag;//過去のフラグを退避 if(Sensor_Digital&0x81){ //マーカセンサ検知時 @@ -334,46 +338,34 @@ else if((SG_Flag==1)&&(SG_Cnt==0)){ //ゴールスタートマーカの時⇒1回目 SG_Cnt=1; if(Sw_Cnt==1){ //1走目のとき - corner_curvature(); //曲率に応じた速度推定処理 - Memory_Enc_Count_A=0; //マーカ間の左タイヤエンコーダパルス数のクリア - Memory_Enc_Count_B=0; //マーカ間の右タイヤエンコーダパルス数のクリア - Row++; + corner_curvature(); //曲率に応じた速度推定処理 }else if(Sw_Cnt==2){ //2走目のとき - second_speed_control(); //記憶情報から目標速度を設定する処理 - Row++; //次のコース情報へ + second_speed_control(); //記憶情報から目標速度を設定する処理 }else{//3走目は定速で高速走行 Target_Speed_A=Default_High_Speed; Target_Speed_B=Default_High_Speed; } }else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目 + Marker_Cnt=Row; //マーカカウント数をLCD表示用に退避させる if(Sw_Cnt==1){ - corner_curvature(); //曲率に応じた速度推定処理 - Memory_Enc_Count_A=0; //マーカ間の左タイヤエンコーダパルス数のクリア - Memory_Enc_Count_B=0; //マーカ間の右タイヤエンコーダパルス数のクリア + corner_curvature(); //曲率に応じた速度推定処理 }else if(Sw_Cnt==2){ //2走目のとき 何もしない - }else; //3走目のとき 何もしない - Marker_Cnt=Row; //マーカカウント数をLCD表示用に退避させる + }else; //3走目のとき 何もしない Machine_Status|=STOP; //機体停止状態へ Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化 SG_Cnt=0; //スタート・ゴールマーカ情報のリセット }else if(Corner_Flag==1){ //コーナマーカの時 if(Sw_Cnt==1){ //1走目のとき - corner_curvature(); //曲率に応じた速度推定処理 - Memory_Enc_Count_A=0; //マーカ間の左タイヤエンコーダパルス数のクリア - Memory_Enc_Count_B=0; //マーカ間の右タイヤエンコーダパルス数のクリア - Row++; + corner_curvature(); //曲率に応じた速度推定処理 }else if(Sw_Cnt==2){ //2走目のとき - second_speed_control(); //記憶情報から目標速度を設定する処理 - Row++; //次のコース情報へ + second_speed_control(); //記憶情報から目標速度を設定する処理 へ }else{//3走目は定速で高速走行 Target_Speed_A=Default_High_Speed; Target_Speed_B=Default_High_Speed; } } }else;//マーカではなく、誤検知だった場合。何もしない - - - + //////マーカ通過後は各種マーカ判定用フラグ、パラメータのリセット処理を行う。 Corner_Flag=0; SG_Flag=0; @@ -463,7 +455,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; //割込み終了時の各種パラメータリセット処理 //エンコーダ関連情報のリセット @@ -492,7 +487,9 @@ if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間 if(Machine_Status&STOP){//機体停止状態の時 - Sw_Cnt++; + Sw_Cnt++; + Memory_Enc_Count_A=0; + Memory_Enc_Count_B=0; wait(0.5); display_print(); //LCDに現在の機体状態を表示 wait(1.5); @@ -502,12 +499,17 @@ Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化 SG_Cnt=0; //スタート・ゴールマーカ情報のリセット Marker_Run_Distance=0;//マーカ通過距離情報リセット - if(Sw_Cnt<=2){ //1,2走目のとき + if(Sw_Cnt==1){ //1走目のとき S_Kp=S_KP_DEFAULT; S_Kd=S_KD_DEFAULT; Target_Speed_A=Default_Speed; Target_Speed_B=Default_Speed; - }else{ //3走目以降のとき + }else if(Sw_Cnt==2){ //2走目のとき + S_Kp=S_KP_DEFAULT; + S_Kd=S_KD_DEFAULT; + second_speed_control(); //記憶情報から目標速度を設定する処理 + } + else{ //3走目以降のとき S_Kp=S_KP_DEFAULT_HIGH; S_Kd=S_KD_DEFAULT_HIGH; Target_Speed_A=Default_High_Speed;