Diff: main.cpp

Revision:

15:c941d06092ec

Parent:

14:aaf2ae25e508

Child:

16:457bb8cc409b

--- a/main.cpp	Sun Oct 04 07:34:44 2015 +0000
+++ b/main.cpp	Mon Oct 05 08:12:15 2015 +0000
@@ -133,40 +133,41 @@
     return val;
 }
 
-void detectPole()
+void detectPole(int scVector)
 {
-    bool btFlag=false,scFlag=false;
-    int vector=TD;
+    int vector=TD,laser=0;
+    bool btFlag=false;
     Blue=0.0f;Green=0.0f,Red=0.8f;
-    double deg=32;
     while(true)
     {
         //スイッチが押された時
-        if(((int)DATA[1]&12)==12&&!btFlag){
+        if((int)DATA[1]&12==12&&!btFlag){
             wait(0.001f);
             Mechanum.putc(64);
             btFlag=true;
             Blue=0.0f;Green=0.8f;Red=0;
-            vector=64;
+            vector=scVector;
         }
         
-        //方向選択
-        else if((int)deg==devideNum)
-        {
-            deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
-            pc.printf("deg%lf\r\n",deg);
-            if((int)deg!=devideNum){
-                vector=(deg<17)?(TNL):(TNR);
-                pc.printf("vector%d\r\n",vector);
-                scFlag=true;
+        //片方のレーザーが反応したとき
+        if(btFlag){
+            if(!(int)DATA[1]&2){
+                laser=1;
+                vector=0;
+            }
+            else if((int)DATA[1]&1){
+                laser=2;
+                vector=0;
             }
         }
-        //レーザーが反応した時
-        else if(!(int)(DATA[1]&0x02)&&btFlag&&scFlag/*!((int)DATA[1]&2==2)&&btFlag*/){
-            vector=64;
+        
+        //もう片方のレーザーが反応した時
+        else if(!(int)DATA[1]&laser&&laser){
+            Mechanum.putc(64);
             break;
         }
         
+        //pc.printf("%d\r\n",DATA[1]);
         Mechanum.putc(vector);
         wait(0.001f);
     }
@@ -221,7 +222,10 @@
     {           
         //ポール検出モード
         if(getBt(btTri)&&getBt(btSph)){
-            detectPole();
+            if(deg<17)
+                detectPole(TNL);
+            else
+                detectPole(TNR);
             //printf("DetectMode\r\n");
         }