島根短大 ライントレーサ制作チーム
Ver1の改良
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 26:7d4150475983
- Parent:
- 25:0dbd9ab9fd86
--- a/main.cpp Sat Nov 23 02:41:50 2019 +0000
+++ b/main.cpp Sat Nov 23 07:09:32 2019 +0000
@@ -11,10 +11,10 @@
//☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define DEFAULT_SPEED 450 //1走目の基本速度
+#define DEFAULT_SPEED 750 //1走目の基本速度
#define LOW_SPEED 450 //2走目の低速速度[mm/sec]
-#define MEDIUM_SPEED 700 //2走目の低速速度[mm/sec]
-#define HIGH_SPEED 800 //2走目の低速速度[mm/sec]
+#define MEDIUM_SPEED 800 //2走目の低速速度[mm/sec]
+#define HIGH_SPEED 1500 //2走目の低速速度[mm/sec]
#define DEFAULT_HIGH_SPEED 950 //3走目の低速速度[mm/sec]
#define STOP_DISTANCE 200000 //停止距離200000[um]⇒20[cm]
#define TURN_POWER 0.6 //コースアウト時の旋回力
@@ -26,8 +26,8 @@
//コースの傷によってマーカ誤検知する場合は値を大きくする。
#define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。
//モータ速度のゲイン関連(むやみに調整しない)
-#define M_KP 0.002f //P(比例)制御成分
-#define M_KD 0.001f //D(微分)制御成分
+#define M_KP 0.001f //P(比例)制御成分
+#define M_KD 0.0005f //D(微分)制御成分
//フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。）
#define S_K1 1.0f //float演算させる値には必ずfを付ける
@@ -37,16 +37,16 @@
//ラインセンサ各種制御成分
#define S_KP_DEFAULT 1.0f //ラインセンサ比例成分。大きいほど曲がりやすい
-#define S_KP_HIGH 0.5f
-#define S_KP_MEDIUM 0.8f
-#define S_KP_LOW 1.2f
+#define S_KP_HIGH 1.0f
+#define S_KP_MEDIUM 1.0f
+#define S_KP_LOW 1.0f
#define S_KP_DEFAULT_HIGH 1.0f
-#define S_KD_DEFAULT 0.5f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
-#define S_KD_HIGH 0.2f
-#define S_KD_MEDIUM 0.4f
-#define S_KD_LOW 0.6f
-#define S_KD_DEFAULT_HIGH 0.5f
+#define S_KD_DEFAULT 1.0f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
+#define S_KD_HIGH 1.0f
+#define S_KD_MEDIUM 1.0f
+#define S_KD_LOW 1.0f
+#define S_KD_DEFAULT_HIGH 1.0f
//////////☆★☆★☆★☆★☆★//////////////
@@ -89,7 +89,7 @@
*/
///////////////////////////////////////
-Serial PC(USBTX,USBRX);
+//Serial PC(USBTX,USBRX);
CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ
CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ
Ticker timer; //タイマ割込み用
@@ -115,7 +115,7 @@
long int Enc_A_Rotate=0,Enc_B_Rotate=0; //エンコーダパルス数。停止時の機体速度調整に使用。
long int Stop_Distance=STOP_DISTANCE; //ストップ時の徐行→停止に使う
/////↓エンコーダパルス数。コース記憶時のエンコーダパルスの蓄積に使用する。
-long int Memory_Enc_Count_A=0,Memory_Enc_Count_B=0;
+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]。
@@ -237,13 +237,14 @@
course_data[Row][1]=(Memory_Enc_Count_B*PULSE_TO_UM);//右タイヤのマーカ間の走行距離を格納
//左右タイヤの走行距離の差分を用いた曲率演算
- if(((course_data[Row][0])>(course_data[Row][1]*1.8f)) || ((course_data[Row][1])>(course_data[Row][0]*1.8f))){//左が右より80%以上早いか20%以上遅いとき
+ if(((course_data[Row][0])>(course_data[Row][1]*4.0f)) || ((course_data[Row][1])>(course_data[Row][0]*4.0f))){//左が右より80%以上早いか20%以上遅いとき
Imaginary_Speed=Low_Speed;//急カーブは低速とする。
- }else if((course_data[Row][0]>(course_data[Row][1]*1.5f)) || (course_data[Row][1]>(course_data[Row][0]*1.5f))){//左が右より50%以上早いか50%以上遅いとき
+ }else if((course_data[Row][0]>(course_data[Row][1]*2.5f)) || (course_data[Row][1]>(course_data[Row][0]*2.5f))){//左が右より50%以上早いか50%以上遅いとき
Imaginary_Speed=Medium_Speed;//中カーブは中速とする。
}else if((course_data[Row][0]>(course_data[Row][1]*1.2f)) || (course_data[Row][1]>(course_data[Row][0]*1.2f))){//左が右より20%以上早いか20%以上遅いとき⇒直線
Imaginary_Speed=High_Speed;//緩いカーブは高速とする。
- }else{
+ }
+ else{
Imaginary_Speed=High_Speed;//直線は高速とする。
}
course_data[Row][2]=Imaginary_Speed; //仮想の演算速度を格納
@@ -292,12 +293,14 @@
//////コース記憶に使用するエンコーダパルス数の蓄積処理
Memory_Enc_Count_A+=Enc_Count_A;
Memory_Enc_Count_B+=Enc_Count_B;
- //////機体停止時の徐行処理に使用するエンコーダパルス数の蓄積
- Enc_A_Rotate+=Enc_Count_A;
- Enc_B_Rotate+=Enc_Count_B;
+
//////機体停止時の徐行用の走行距離演算
if(Machine_Status&STOP){//機体停止状態の時
+ if(Stop_Distance>=STOP_DISTANCE){
+ Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積
+ Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積
+ }
if(Enc_A_Rotate<-6400)Enc_A_Rotate=-6400;
if(Enc_A_Rotate>6400)Enc_A_Rotate=6400;
if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400;
@@ -314,9 +317,7 @@
if((Corner_Flag==1)&&(SG_Flag==1));//交差点通過中。何もしない
}else Marker_Pass_Flag=0;//マーカ通過終了
- if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
- Marker_Run_Distance+=(Distance_A+Distance_B)/2;
- }
+
//マーカ通過後、マーカ種類判別
if((Old_Marker_Pass_Flag==1)&&(Marker_Pass_Flag==0)){//マーカ通過後
if(Marker_Run_Distance>MARKER_WIDTH){ //マーカ幅がもっともらしいとき
@@ -324,16 +325,20 @@
else if((SG_Flag==1)&&(SG_Cnt==0)){ //ゴールスタートマーカの時⇒1回目
SG_Cnt=1;
if(Sw==0){ //1走目のとき
+ Memory_Enc_Count_A=0;
+ Memory_Enc_Count_B=0;
corner_curvature(); //曲率に応じた速度推定処理
Row++;
- }else if(Sw==1){ //2走目のとき
- ++Row;//一つ先のコース情報を読む。
- second_speed_control();//記憶情報から目標速度を設定する処理
+ }else if(Sw==1){ //2走目のとき
+ ++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回目
+ corner_curvature(); //曲率に応じた速度推定処理
+ Row++;
Machine_Status|=STOP; //機体停止状態へ
Row=0; //コース記憶用配列の行情報[通過マーカ情報]の初期化
SG_Cnt=0;//スタート・ゴールマーカ情報のリセット
@@ -417,7 +422,10 @@
motor_a=0;
motor_b=0;
}
-
+ if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
+ Marker_Run_Distance+=(Distance_A+Distance_B)/2;
+ }
+
//割込み終了時の各種パラメータリセット処理
//エンコーダ関連情報のリセット
encoder_a.Set(0);//エンコーダクラスのパルス数情報のリセット
@@ -435,8 +443,7 @@
lcd.locate(0,0);
lcd.print("STOP");
lcd.locate(0,1);
- sprintf(Speed_Str,"%04d",Medium_Speed);
- lcd.print(Speed_Str);
+
while(1){
Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避
@@ -470,8 +477,7 @@
lcd.locate(0,1);
lcd.print(" ");
- lcd.locate(0,1);
- lcd.print(Speed_Str);
+
if(Sw==0){
Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整
sprintf(Speed_Str,"%04d",Default_Speed);//速度情報文字列変換
@@ -481,11 +487,13 @@
Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);
sprintf(Speed_Str,"%04d",Low_Speed);//速度情報文字列変換
}
- else if(Sw==2)//3走目。加減速による超高速走行
+ else if(Sw==2){//3走目。加減速による超高速走行
Default_High_Speed=DEFAULT_HIGH_SPEED+(Enc_B_Rotate/16);
- sprintf(Speed_Str,"%04d",Default_High_Speed);//速度情報文字列変換
+ sprintf(Speed_Str,"%04d",Default_High_Speed);//速度情報文字列変換
+ }
}
-
+ lcd.locate(0,1);
+ lcd.print(Speed_Str);
if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間
if(Machine_Status&STOP){//機体停止状態の時