ドローンを搭載した機体の交差点認識を行い、ゴール地点でSTOPするプログレラム
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG_Ver1
Diff: main.cpp
- Revision:
- 17:8d0dc3258cc4
- Parent:
- 16:d7444aa6afe7
- Child:
- 18:c13fb85f1250
--- a/main.cpp Mon Nov 25 10:23:01 2019 +0000 +++ b/main.cpp Thu Dec 19 00:07:50 2019 +0000 @@ -97,7 +97,7 @@ unsigned char Old_Machine_Status=0x00; //過去の機体状態 int Marker_Pass_Flag = 0; int Old_Marker_Pass_Flag=0; -int Corner_Flag=0; +int Cross_Flag2=0; int SG_Flag=0; int SG_Cnt=0; int Cross_Flag=0; @@ -116,9 +116,6 @@ void sensor_digital_read(){//8つのフォトリフレクタの入力を8ビットのデジタルパターンに変換 Sensor_Cnt=0; Old_Sensor_Digital=Sensor_Digital; - if(S1_Data>Gray){ - Sensor_Digital |= 0x80; //7ビット目のみセット (1にする。) - }else Sensor_Digital &= 0x7F; //7ビット目のみマスク(0にする。) if(S2_Data>Gray){ Sensor_Digital |= 0x40; //6ビット目のみセット (1にする。) Sensor_Cnt++; @@ -181,11 +178,11 @@ //各種マーカの検知 Old_Marker_Pass_Flag=Marker_Pass_Flag;//過去のフラグを退避 - if(Sensor_Digital&0x81){ //マーカセンサ検知時 + if(Sensor_Digital&0x41){ //マーカセンサ検知時 Marker_Pass_Flag=1; //マーカ通過中フラグをON - if(Sensor_Digital&0x80)Corner_Flag=1; //コーナセンサの検知 + if((Sensor_Digital&0x40)/*&&(Sensor_Digital&0x20)&&(Sensor_Digital&0x04)&&(Sensor_Digital&0x02)*/)Cross_Flag2=1; // if(Sensor_Digital&0x01)SG_Flag=1; //スタートゴールセンサの検知 - if((Corner_Flag==1)&&(SG_Flag==1))Cross_Flag=1;//交差点通過中。何もしない + if((Cross_Flag2==1)&&(SG_Flag==1))Cross_Flag=1;//交差点通過中。何もしない }else Marker_Pass_Flag=0;//マーカ通過終了 //マーカ通過後、マーカ種類判別 @@ -195,15 +192,15 @@ else if((SG_Flag==1)&&(SG_Cnt==0)){//ゴールスタートマーカの時⇒1回目 SG_Cnt=1; }else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目 - //Machine_Status|=STOP; //機体停止状態へ + Machine_Status|=STOP; //機体停止状態へ SG_Cnt=0; - }else if(Corner_Flag==1){//コーナマーカの時 + }else if(Cross_Flag2==1){//コーナマーカの時 //地区大会では何もしない } }else{//マーカではなく、誤検知だった場合。 //何もしない } - Corner_Flag=0; + Cross_Flag2=0; SG_Flag=0; Cross_Flag=0; Marker_Run_Distance=0;//マーカ通過距離情報リセット @@ -291,6 +288,7 @@ }else if(Sensor_Cnt>=CROSS_JUDGE){//交差点通過中 motor_a=-0.3;//交差点なので控えめの速度で直進 motor_b=-0.3; + //Cross_Flag=1; } else{ motor_a=-Motor_A_Pwm; @@ -304,6 +302,7 @@ }else{ motor_a=0; motor_b=0; + //Cross_Flag=0; } } } @@ -359,7 +358,7 @@ Machine_Status&=0x7F;//ストップ状態解除 }else{//機体走行中であったとき //各種フラグのクリア - Corner_Flag=0; + Cross_Flag2=0; SG_Flag=0; Cross_Flag=0; Marker_Run_Distance=0;//マーカ通過距離情報リセット