Diff: main.cpp

Revision:

16:457bb8cc409b

Parent:

15:c941d06092ec

Child:

17:1942c2ba89a0

--- a/main.cpp	Mon Oct 05 08:12:15 2015 +0000
+++ b/main.cpp	Mon Oct 05 10:10:45 2015 +0000
@@ -136,38 +136,50 @@
 void detectPole(int scVector)
 {
     int vector=TD,laser=0;
-    bool btFlag=false;
+    bool btFlag=false,canFlag=false;
     Blue=0.0f;Green=0.0f,Red=0.8f;
     while(true)
-    {
+    {    
+        if(getBt(btTri)&&getBt(btSph)&&canFlag)
+            break;
+        else if(!(getBt(btTri)&&getBt(btSph)))
+            canFlag=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;
+            Blue=0;Green=0.8;Red=0;
             vector=scVector;
+            //pc.printf("%d\r\n",scVector);
         }
         
         //片方のレーザーが反応したとき
-        if(btFlag){
-            if(!(int)DATA[1]&2){
+        if(btFlag&&!laser){
+            if(!(((int)DATA[1]&2)==2)){
                 laser=1;
+                Red=0.8;
                 vector=0;
             }
-            else if((int)DATA[1]&1){
+            else if(!(((int)DATA[1]&1)==1)){
                 laser=2;
+                Red=0.8;
                 vector=0;
             }
         }
         
         //もう片方のレーザーが反応した時
-        else if(!(int)DATA[1]&laser&&laser){
+        else if(!(((int)DATA[1]&laser)==laser)&&laser){
+            Green=0;Red=0.8;Blue=0.8;
             Mechanum.putc(64);
+            wait(0.001f);
+            Mechanum.putc(162);
             break;
         }
         
         //pc.printf("%d\r\n",DATA[1]);
+        deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
         Mechanum.putc(vector);
         wait(0.001f);
     }
@@ -222,11 +234,11 @@
     {           
         //ポール検出モード
         if(getBt(btTri)&&getBt(btSph)){
-            if(deg<17)
-                detectPole(TNL);
+            if(deg>devideNum-1);
+            else if(deg<17)
+                detectPole(TNR);
             else
-                detectPole(TNR);
-            //printf("DetectMode\r\n");
+                detectPole(TNL);
         }
         
         //射出