Diff: main.cpp

Revision:

6:c2c31bc971ad

Parent:

5:3d68334aab20

Child:

7:7a0aee1477d9

--- a/main.cpp	Thu Jan 08 05:08:34 2015 +0000
+++ b/main.cpp	Fri Jan 09 06:59:18 2015 +0000
@@ -15,7 +15,7 @@
  *
  *　スイッチ4つとスタートスイッチで処理を実行
  *  
- * キッカーのスイッチがどこかに入る
+ * キッカー用のDigitalOutがどこかに入る
  *
  ************************* 
  * 
@@ -112,19 +112,30 @@
     val = Sensor.read(ADDRESS_L|1, data_l, 3);
     Led[0] = !val;
     
+    if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
+        if((data_r[0] == 0)&&(data_l[0] == 0)){
+            return 12;
+        }
+        if(data_r[0] == 0){
+            return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6;
+        }
+        return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6;
+       
+    }
+    
     if(data_r[2]>data_l[2]){
         if(data_r[2]>data_l[1]){   
             return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6;
-        }else{
-            return data_r[0]/6*12*12 +(data_l[0]/6+6)*12+ data_r[0]%6;
         }
+        
+        return data_r[0]/6*12*12 +(data_l[0]/6+6)*12+ data_r[0]%6;
+        
     }else{
         if(data_l[2]>data_r[1]){   
             return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6;
-        }else{
-            return (data_l[0]/6+6)*12*12 +(data_r[0]/6)*12+ (data_l[0]%6+6);
         }
         
+        return (data_l[0]/6+6)*12*12 +(data_r[0]/6)*12+ (data_l[0]%6+6);      
     }
     
 }
@@ -135,6 +146,38 @@
     Slave側はIRを要求した場合IRのみを返してくるとする．
     irは値が大きいほうが近いと仮定する
     
+    */
+    char data_r[3],data_l[3];
+    bool val;
+    Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
+    
+    val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
+    Led[0] = val;
+    wait_ms(5);
+    val = Sensor.read(ADDRESS_L|1, data_l, 3);
+    Led[0] = !val;
+    
+    if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
+        if((data_r[0] == 0)&&(data_l[0] == 0)){
+            return 12;
+        }
+        if(data_r[0] == 0){
+            return data_l[0]/6+6;
+        }
+        return data_r[0]/6;  
+    }
+
+    if(data_r[2]>data_l[2]){   
+        return data_r[0]/6;
+    }
+    return data_l[0]/6+6;
+    
+}
+uint8_t IrReceiveM(){
+    /*値解析版　一番近い場所の値を返す（位置はとりあえずない）
+    Slave側はIRを要求した場合IRのみを返してくるとする．
+    irは値が大きいほうが近いと仮定する
+    
     
     */
     char data_r[3],data_l[3];
@@ -147,31 +190,17 @@
     val = Sensor.read(ADDRESS_L|1, data_l, 3);
     Led[0] = !val;
 
-    if(data_r[2]>data_l[2]){   
-        return data_r[0]/6;
-    
-    }else{
-        return data_l[0]/6+6;
+    if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
+        if((data_r[0] == 0)&&(data_l[0] == 0)){
+            return 255;
+        }
+        if(data_r[0] == 0){
+            return data_l[1];
+        }
+        return data_r[1];   
     }
     
-}
-uint8_t IrReceiveM(){
-    /*値解析版　一番大きい場所の値を返す（位置はとりあえずない）
-    Slave側はIRを要求した場合IRのみを返してくるとする．
-    irは値が大きいほうが近いと仮定する
     
-    
-    */
-    char data_r[3],data_l[3];
-    bool val;
-    Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
-    
-    val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
-    Led[0] = val;
-    wait_ms(5);
-    val = Sensor.read(ADDRESS_L|1, data_l, 3);
-    Led[0] = !val;
-
     if(data_r[2]>data_l[2]){       
         return data_r[1];
         
@@ -197,14 +226,24 @@
     wait_ms(5);
     val = Sensor.read(ADDRESS_L|1, data_l, 3);
     Led[0] = !val;
-
+    
+    if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
+        if((data_r[0] == 0)&&(data_l[0] == 0)){
+            return 12<<8+255;
+        }
+        if(data_r[0] == 0){
+            return (data_l[0]/6+6)<<8 + data_l[1];
+        }
+        return data_r[0]/6<<8 + data_r[1];
+    }
+    
+    
     if(data_r[2]>data_l[2]){       
         return data_r[0]/6<<8 + data_r[1];
         
-    }else{ 
-        return ((data_l[0]/6)+6) <<8 + data_l[1];
-        
     }
+    return ((data_l[0]/6)+6) <<8 + data_l[1];
+    
     
 }
 uint8_t  LineReceive(){
@@ -485,7 +524,6 @@
 
 void SetUp(int *compass_def){
     /*初期化*/
-    int Compass;
     Kick = 0;
     Sw.mode(PullUp);
     Motor.baud(115200);                             //ボーレート設定
@@ -498,14 +536,7 @@
 
     hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     *compass_def = (hmc6352.sample() / 10);
-    /*
-    前を向くように設定をするべき→最初っから前向いてるし
-    アウトオブバウンズからの復帰時に反対を向けてスタートなので必要。
-    */
-    while(1){
-        Compass = ((hmc6352.sample() / 10) + 540 - *compass_def) % 360;
-        if(Compass == 3) break;//前を向いたら抜ける。
-    }
+    
     Lcd.cls();
     
     /*初期化終了*/
@@ -609,6 +640,14 @@
     
     StartLoop(); /*loop stat, switch push end*/
     
+    /*
+    前を向くように設定をするべき
+    アウトオブバウンズからの復帰時に反対を向けてスタートなので必要。
+    */
+    while(1){//前を向かせるwhile
+        Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
+        if(Compass == 3) break;//前を向いたら抜ける。
+    }
     while(1) {
         
         Line = LineReceive();