![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
本当の空飛ぶ脅威 シャア・アズナブル
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG_Ver1
Diff: main.cpp
- Revision:
- 14:584e778a20ea
- Parent:
- 13:97be8e29ae50
- Child:
- 15:9e234fb2d07a
--- a/main.cpp Thu Aug 29 01:07:15 2019 +0000 +++ b/main.cpp Sun Sep 01 06:10:26 2019 +0000 @@ -6,13 +6,14 @@ //☆★☆★各種パラメータ調整箇所☆★☆★☆★ -#define DEFAULT_SPEED 800 //1走目の基本速度[mm/sec] -#define TURN_POWER 0.47 //コースアウト時の旋回力 +#define DEFAULT_SPEED 700 //1走目の基本速度[mm/sec] +#define STOP_DISTANCE 200000 //停止距離200000[um]⇒20[cm] +#define TURN_POWER 0.3 //コースアウト時の旋回力 #define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um] #define INTERRUPT_TIME 1000 //割りこみ周期[us] -#define DEFAULT_GRAY 0.5f //フォトリフレクタデジタル入力の閾値 +#define DEFAULT_GRAY 0.3f //フォトリフレクタデジタル入力の閾値 //シリアル通信でSensor_Digital値を確認し調整する。 -#define MARKER_WIDTH 15 //マーカ幅[mm](ビニルテープ幅19[mm]以内) +#define MARKER_WIDTH 13000 //マーカ幅[um](ビニルテープ幅19[um]以内) //コースの傷によってマーカ誤検知する場合は値を大きくする。 #define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。 //モータ速度のゲイン関連(むやみに調整しない) @@ -22,11 +23,11 @@ //フォトリフレクタのゲイン(外側に行くにつれ値を何倍させたいか調整する。) #define S_K1 1.0f //float演算させる値には必ずfを付ける #define S_K2 2.0f //2倍 -#define S_K3 4.0f //4倍 +#define S_K3 3.0f //4倍 //ラインセンサ各種制御成分 -#define S_KP 1.07f //ラインセンサ比例成分。大きいほど曲がりやすい -#define S_KD 0.7f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。 +#define S_KP 0.5f //ラインセンサ比例成分。大きいほど曲がりやすい +#define S_KD 0.5f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。 //////////☆★☆★☆★☆★☆★////////////// @@ -77,7 +78,7 @@ 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 int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] long int Marker_Run_Distance=0; long int Speed_A=0, Speed_B=0; //現在速度 @@ -178,17 +179,14 @@ //機体状態の取得 Machine_Status_Set(); - - //交差点の認識 - if(Sensor_Cnt>=CROSS_JUDGE )Cross_Flag=1;//ラインセンサ4つ以上検知状態の時は交差点を示す。 - + //各種マーカの検知 Old_Marker_Pass_Flag=Marker_Pass_Flag;//過去のフラグを退避 if(Sensor_Digital&0x81){ //マーカセンサ検知時 Marker_Pass_Flag=1; //マーカ通過中フラグをON if(Sensor_Digital&0x80)Corner_Flag=1; //コーナセンサの検知 if(Sensor_Digital&0x01)SG_Flag=1; //スタートゴールセンサの検知 - if((Corner_Flag==1)&&(SG_Flag==1));//交差点通過中。何もしない + if((Corner_Flag==1)&&(SG_Flag==1))Cross_Flag=1;//交差点通過中。何もしない }else Marker_Pass_Flag=0;//マーカ通過終了 //マーカ通過後、マーカ種類判別 @@ -224,22 +222,29 @@ ////モータ現在速度の取得 Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得 Enc_Count_B=-encoder_b.Get(); - Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納 + Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をum単位で格納 Distance_B=(Enc_Count_B*PULSE_TO_UM); Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s] Speed_B=(Distance_B*1000)/INTERRUPT_TIME; if(Machine_Status&STOP){//機体停止状態の時 - Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積 + 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; - Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積 if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400; if(Enc_B_Rotate>6400)Enc_B_Rotate=6400; + if((Distance_A>=0)&&(Distance_B>=0))Stop_Distance+=(Distance_A+Distance_B)/2; + if(Stop_Distance<0)Stop_Distance=0; + if(Stop_Distance>STOP_DISTANCE)Stop_Distance=STOP_DISTANCE; } if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。 Marker_Run_Distance+=(Distance_A+Distance_B)/2; } + + //エンコーダ関連情報のリセット Distance_A=0; Distance_B=0; @@ -284,7 +289,7 @@ }else if(Machine_Status&RUN_COURSE_ROUT){ //右端センサ振り切れた時 motor_a=-(TURN_POWER); //右旋回 motor_b=-(-TURN_POWER); - }else if(Cross_Flag==1){//交差点通過中 + }else if(Sensor_Cnt>=CROSS_JUDGE){//交差点通過中 motor_a=-0.3;//交差点なので控えめの速度で直進 motor_b=-0.3; } @@ -293,8 +298,14 @@ motor_b=-Motor_B_Pwm; } }else{//停止状態の時はモータへの出力は無効 - motor_a=0; - motor_b=0; + if(Stop_Distance<STOP_DISTANCE){ + motor_a=-Motor_A_Pwm/4; + motor_b=-Motor_B_Pwm/4; + + }else{ + motor_a=0; + motor_b=0; + } } } @@ -311,11 +322,15 @@ while(1){ Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避 Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得 - if((!(Old_Machine_Status&=STOP))&&(Machine_Status&=STOP)){//走行終了時 + PC.printf("Sensor_Digital:%02x\tMachine_Status:%02x\tSG_Cnt:%d\r\n",Sensor_Digital,Machine_Status,SG_Cnt); + if((!(Old_Machine_Status&STOP))&&(Machine_Status&STOP)){//走行終了時 lcd.locate(0,0); lcd.print(" "); lcd.locate(0,0); - lcd.print("STOP"); + lcd.print("STOP"); + wait(5); + Gray=DEFAULT_GRAY; + Machine_Speed=DEFAULT_SPEED; } if(Machine_Status&STOP){//機体停止状態の時 @@ -341,6 +356,7 @@ lcd.locate(0,0); lcd.print("GO!!"); wait(2); + Stop_Distance=0; Machine_Status&=0x7F;//ストップ状態解除 }else{//機体走行中であったとき //各種フラグのクリア