島根短大 ライントレーサ制作チーム
最終調整
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.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))){//左右の回転差が５0%以下の時
+ }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;