Diff: main.cpp

Revision:

18:c13fb85f1250

Parent:

17:8d0dc3258cc4

--- a/main.cpp	Thu Dec 19 00:07:50 2019 +0000
+++ b/main.cpp	Fri Dec 20 01:10:55 2019 +0000
@@ -55,6 +55,11 @@
 AnalogIn    s6(A2);
 AnalogIn    s7(A1);
 AnalogIn    s8(A0);
+
+/////ドローン定義
+//DigitalOut    drone(D3);
+
+//int Drone = drone;
 ///////////////////////////////////////
 Serial      PC(USBTX,USBRX);
 CRotaryEncoder encoder_a(D1,D0);    //モータAのエンコーダ
@@ -67,7 +72,7 @@
 //使用変数の定義
 int    Sw_Ptn;
 int    Old_Sw_Ptn;
-double S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; 
+double S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; 
 double  All_Sensor_Data;      //ラインセンサ総データ量
 double Sensor_Diff[2]={0,0}; //ラインセンサ偏差
 double Sensor_P =0.0f;        //ラインセンサP(比例成分)制御量
@@ -101,8 +106,10 @@
 int SG_Flag=0;
 int SG_Cnt=0;
 int Cross_Flag=0;
+
+
 void sensor_analog_read(){
-    S1_Data=0;
+    
     S2_Data=s2.read();
     S3_Data=s3.read();
     S4_Data=s4.read();
@@ -178,11 +185,11 @@
   
     //各種マーカの検知
     Old_Marker_Pass_Flag=Marker_Pass_Flag;//過去のフラグを退避
-    if(Sensor_Digital&0x41){                   //マーカセンサ検知時
+    if(Sensor_Digital&0x01){                   //マーカセンサ検知時
         Marker_Pass_Flag=1;                     //マーカ通過中フラグをON
-        if((Sensor_Digital&0x40)/*&&(Sensor_Digital&0x20)&&(Sensor_Digital&0x04)&&(Sensor_Digital&0x02)*/)Cross_Flag2=1;  //
+        if(Cross_Flag==1);  //
         if(Sensor_Digital&0x01)SG_Flag=1;      //スタートゴールセンサの検知
-        if((Cross_Flag2==1)&&(SG_Flag==1))Cross_Flag=1;//交差点通過中。何もしない    
+        if((Cross_Flag==1)&&(SG_Flag==1))Cross_Flag=1;//交差点通過中。何もしない    
     }else Marker_Pass_Flag=0;//マーカ通過終了       
 
     //マーカ通過後、マーカ種類判別    
@@ -194,13 +201,10 @@
             }else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目
                 Machine_Status|=STOP;           //機体停止状態へ
                 SG_Cnt=0;    
-            }else if(Cross_Flag2==1){//コーナマーカの時
-                //地区大会では何もしない                
             }
         }else{//マーカではなく、誤検知だった場合。
             //何もしない
         }
-        Cross_Flag2=0;
         SG_Flag=0;
         Cross_Flag=0;
         Marker_Run_Distance=0;//マーカ通過距離情報リセット
@@ -286,9 +290,10 @@
             motor_a=-(TURN_POWER);  //右旋回    
             motor_b=-(-TURN_POWER);
         }else if(Sensor_Cnt>=CROSS_JUDGE){//交差点通過中
+            Cross_Flag=1;
             motor_a=-0.3;//交差点なので控えめの速度で直進
             motor_b=-0.3;
-            //Cross_Flag=1;
+            
         }
         else{
             motor_a=-Motor_A_Pwm;
@@ -302,7 +307,7 @@
         }else{
             motor_a=0;
             motor_b=0;
-            //Cross_Flag=0;
+            Cross_Flag=0;
         }        
     }                       
 }
@@ -326,6 +331,8 @@
             lcd.print("     ");
             lcd.locate(0,0);
             lcd.print("STOP");
+
+  //          Drone=0;
             wait(5);
             Gray=DEFAULT_GRAY;
             Machine_Speed=DEFAULT_SPEED;                            
@@ -354,15 +361,18 @@
                 lcd.locate(0,0);
                 lcd.print("GO!!");
                 wait(2);
+
+    //            Drone=1;
                 Stop_Distance=0;
                 Machine_Status&=0x7F;//ストップ状態解除
             }else{//機体走行中であったとき
                 //各種フラグのクリア              
-                Cross_Flag2=0;
+                Cross_Flag=0;
                 SG_Flag=0;
                 Cross_Flag=0;
                 Marker_Run_Distance=0;//マーカ通過距離情報リセット
-                                
+
+      //          Drone=0;                
                 Machine_Status |= STOP;//機体停止状態にする。
                 lcd.locate(0,0);
                 lcd.print("     ");