Diff: main.cpp

Revision:

9:489a085d1703

Parent:

8:b2ead9ecd6cd

Child:

10:ab1339498e09

--- a/main.cpp	Thu Sep 17 04:08:04 2015 +0000
+++ b/main.cpp	Thu Sep 17 04:58:57 2015 +0000
@@ -133,10 +133,10 @@
 
 void detectPole()
 {
-    bool btFlag=false;
+    bool btFlag=false,scFlag=false;
     int vector=TD;
     Blue=0.0f;Green=0.0f,Red=0.8f;
-    double deg;
+    double deg=32;
     while(true)
     {
         //スイッチが押された時
@@ -145,21 +145,20 @@
             Mechanum.putc(64);
             btFlag=true;
             Blue=0.0f;Green=0.8f;Red=0;
-            vector=TNL;
-            
-            /*//左右どっちかが選択されるまで停止
-            do{
-                //deg更新
-                deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
-                //ニュートラル以外なら
-                if(deg!=64)
-                    vector=(deg<17)?(TNR):(TNL);//左右選択
-            }while(deg==64);*/
+            vector=64;
         }
-        
+        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;
+            }
+        }
         //レーザーが反応した時
-        else if(!(int)(DATA[1]&0x02)&&btFlag/*!((int)DATA[1]&2==2)&&btFlag*/){
-            pc.printf("raser%d\r\n",(int)DATA[1]);
+        else if(!(int)(DATA[1]&0x02)&&btFlag&&scFlag/*!((int)DATA[1]&2==2)&&btFlag*/){
             vector=64;
             break;
         }