![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
本当の空飛ぶ脅威 シャア・アズナブル
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG_Ver1
Diff: main.cpp
- Revision:
- 10:e1eb10665472
- Parent:
- 9:1c28fcc1e9b8
- Child:
- 11:2cd6f8be124e
diff -r 1c28fcc1e9b8 -r e1eb10665472 main.cpp --- a/main.cpp Wed Aug 28 07:53:37 2019 +0000 +++ b/main.cpp Wed Aug 28 09:44:41 2019 +0000 @@ -1,6 +1,4 @@ -////ライントレースサンプルver5 -///マーカ検知機能の追加 -///スタートマーカ検知後、ゴールマーカの検知で停止するプログラム +////ライントレースサンプルver7 #include "mbed.h" #include "CRotaryEncoder.h" #include "TB6612.h" @@ -14,9 +12,9 @@ #define TURN_POWER 0.5 //コースアウト時の旋回力 #define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um] #define INTERRUPT_TIME 1000 //割りこみ周期[us] -#define GRAY 0.2f //フォトリフレクタデジタル入力の閾値 +#define GRAY 0.6f //フォトリフレクタデジタル入力の閾値 //シリアル通信でSensor_Digital値を確認し調整する。 -#define MARKER_WIDTH 10 //マーカ幅[mm](ビニルテープ幅19[mm]以内) +#define MARKER_WIDTH 15 //マーカ幅[mm](ビニルテープ幅19[mm]以内) //コースの傷によってマーカ誤検知する場合は値を大きくする。 #define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。 //モータ速度のゲイン関連(むやみに調整しない) @@ -47,6 +45,10 @@ #define SECOND_RUN 0x02 //機体停止状態 #define TUARD_RUN 0x01 //機体設定モード +#define FIRST 1 +#define SECOND 2 +#define THIRD 3 + //デジタル入力オブジェクト定義 DigitalIn push_sw(D13); /////アナログ入力オブジェクト定義////////// @@ -70,7 +72,7 @@ //使用変数の定義 int Sw_Ptn; int Old_Sw_Ptn; -int Push_Cnt; +int Run_Mode=FIRST; double S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; double All_Sensor_Data; //ラインセンサ総データ量 double Sensor_Diff[2]={0,0}; //ラインセンサ偏差 @@ -78,10 +80,11 @@ double Sensor_D =0.0f; //ラインセンサD(微分成分)制御量 double Sensor_PD=0.0f; //ラインセンサP,D成分の合計 long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納 +long int Enc_B_Rotate=0; long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] long int Marker_Run_Distance=0; long int Speed_A=0, Speed_B=0; //現在速度 -long int Default_Speed=0; +long int Default_Speed=FIRST_SPEED; 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}; // @@ -226,7 +229,13 @@ Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納 Distance_B=(Enc_Count_B*PULSE_TO_UM); Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s] - Speed_B=(Distance_B*1000)/INTERRUPT_TIME; + Speed_B=(Distance_B*1000)/INTERRUPT_TIME; + + if(Machine_Status&STOP){//機体停止状態の時 + Enc_B_Rotate+=Enc_Count_B;//モード設定用に右エンコーダ値の蓄積 + if(Enc_B_Rotate<0)Enc_B_Rotate=0; + if(Enc_B_Rotate>6400)Enc_B_Rotate=6400; + } if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。 Marker_Run_Distance+=(Distance_A+Distance_B)/2; } @@ -294,42 +303,75 @@ timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート lcd.cls();//表示クリア lcd.locate(0,0); - lcd.print("STOP"); + lcd.print("STOP"); + lcd.locate(0,1); + lcd.print("FIRST"); while(1){ Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避 Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得 + if((!(Old_Machine_Status&=STOP))&&(Machine_Status&=STOP)){//走行終了時 + lcd.locate(0,0); + lcd.print(" "); + lcd.locate(0,0); + lcd.print("STOP"); + } + if(Machine_Status&STOP){//機体停止状態の時 + if((Enc_B_Rotate/1600)<1){//右エンコーダが1回転未満であったら + if(Run_Mode!=FIRST){ + lcd.cls();//表示クリア + lcd.locate(0,0); + lcd.print("STOP"); + lcd.locate(0,1); + lcd.print("FIRST"); + Run_Mode=FIRST; + Default_Speed=FIRST_SPEED; + } + }else if(((Enc_B_Rotate/1600)>=1)&&((Enc_B_Rotate/1600)<2)){//右エンコーダが1回転以上、2回転未満であったら + if(Run_Mode!=SECOND){ + lcd.cls();//表示クリア + lcd.locate(0,0); + lcd.print("STOP"); + lcd.locate(0,1); + lcd.print("SECOND");//2走目のモードであることを示す。 + Run_Mode=SECOND; + Default_Speed=SECOND_SPEED; + } + }else if(((Enc_B_Rotate/1600)>=2)&&((Enc_B_Rotate/1600)<3)){//右エンコーダが2回転以上、3回転未満であったら + if(Run_Mode!=THIRD){ + lcd.cls();//表示クリア + lcd.locate(0,0); + lcd.print("STOP"); + lcd.locate(0,1); + lcd.print("THIRD");//2走目のモードであることを示す。 + Run_Mode=THIRD; + Default_Speed=THIRD_SPEED; + } + } + + } + if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間 - wait(0.2);//チャタリング動作落ち着くまでの待ち時間 - Push_Cnt++; - //各種フラグのクリア - Corner_Flag=0; - SG_Flag=0; - Cross_Flag=0; - Marker_Run_Distance=0;//マーカ通過距離情報リセット - if(Push_Cnt>=3)Push_Cnt=3;//スイッチ押下回数の上限は3 - if(Push_Cnt==1){ - lcd.cls();//表示クリア + if(Machine_Status&STOP){//機体停止状態の時 + lcd.locate(0,0); + lcd.print(" "); lcd.locate(0,0); - lcd.print("FIRST"); - wait(2); - Machine_Status&=0x7F;//STOP状態の解除 - Default_Speed=FIRST_SPEED; - }else if(Push_Cnt==2){ - lcd.cls();//表示クリア + lcd.print("GO!!"); + wait(2); + Machine_Status&=0x7F;//ストップ状態解除 + }else{//機体走行中であったとき + //各種フラグのクリア + Corner_Flag=0; + SG_Flag=0; + Cross_Flag=0; + Marker_Run_Distance=0;//マーカ通過距離情報リセット + + Machine_Status |= STOP;//機体停止状態にする。 lcd.locate(0,0); - lcd.print("SECOND"); - wait(2); - Machine_Status&=0x7F;//STOP状態の解除 - Default_Speed=SECOND_SPEED; - }else if(Push_Cnt==3){ - lcd.cls();//表示クリア + lcd.print(" "); lcd.locate(0,0); - lcd.print("THIRD"); - wait(2); - Machine_Status&=0x7F;//STOP状態の解除 - Default_Speed=THIRD_SPEED; - } + lcd.print("STOP"); + } } } } \ No newline at end of file