Diff: main.cpp

Revision:

17:1942c2ba89a0

Parent:

16:457bb8cc409b

Child:

18:e3591b0b9e40

diff -r 457bb8cc409b -r 1942c2ba89a0 main.cpp
--- a/main.cpp	Mon Oct 05 10:10:45 2015 +0000
+++ b/main.cpp	Wed Oct 07 10:49:50 2015 +0000
@@ -9,17 +9,18 @@
 #define btSph 5
 #define btCro 6
 #define btSqa 7
-#define presUp 0
-#define presRight 1
-#define presDown 2
-#define presLeft 3
+#define btSel 0
+#define presUp 4
+#define presRight 5
+#define presDown 6
+#define presLeft 7
 #define devideNum 32
 #define ED 132
 #define TD 144//208
 #define EL 134
 #define TL 200
 #define TNR 138//204
-#define TNL 135
+#define TNL 150
 #define debugFlag 1
 #define stopCount 33
 
@@ -136,11 +137,11 @@
 void detectPole(int scVector)
 {
     int vector=TD,laser=0;
-    bool btFlag=false,canFlag=false;
+    bool btFlag=false,canFlag=false,breakFlag=false;
     Blue=0.0f;Green=0.0f,Red=0.8f;
     while(true)
     {    
-        if(getBt(btTri)&&getBt(btSph)&&canFlag)
+        if((getBt(btTri)&&getBt(btSph)&&canFlag)||breakFlag)
             break;
         else if(!(getBt(btTri)&&getBt(btSph)))
             canFlag=true;
@@ -152,7 +153,6 @@
             btFlag=true;
             Blue=0;Green=0.8;Red=0;
             vector=scVector;
-            //pc.printf("%d\r\n",scVector);
         }
         
         //片方のレーザーが反応したとき
@@ -172,16 +172,17 @@
         //もう片方のレーザーが反応した時
         else if(!(((int)DATA[1]&laser)==laser)&&laser){
             Green=0;Red=0.8;Blue=0.8;
+            wait(0.0125f);
             Mechanum.putc(64);
-            wait(0.001f);
-            Mechanum.putc(162);
-            break;
+            vector=162;
+            breakFlag=true;
         }
         
         //pc.printf("%d\r\n",DATA[1]);
-        deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
+        //deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
+        //pc.printf("%d\r\n",scVector);
         Mechanum.putc(vector);
-        wait(0.001f);
+        wait(0.0126f);
     }
     btFlag=false;
     Blue=0;Green=0;Red=0;   
@@ -224,7 +225,7 @@
     double gValue=0,bValue=0,rValue=0;
     DigitalOut l(PB_2);
     int sqf=0;
-    double deg;
+    double deg=32;
     int val;
     i2c.frequency(400000); 
     setFCLED(0,0.8,0.8);
@@ -235,10 +236,14 @@
         //ポール検出モード
         if(getBt(btTri)&&getBt(btSph)){
             if(deg>devideNum-1);
-            else if(deg<17)
+            else if(deg<17){
+                //pc.printf("R\r\n");
                 detectPole(TNR);
-            else
+            }
+            else{
+                //pc.printf("L\r\n");
                 detectPole(TNL);
+            }
         }
         
         //射出
@@ -318,6 +323,7 @@
         //タッチセンサ
         if(((int)DATA[1]&12)==12){
             bValue=0.0f;gValue=0.8f;rValue=0;
+            //Mechanum.putc(162);
         }
         else{
             bValue=0.8f;gValue=0;rValue=0;
@@ -338,9 +344,11 @@
             obonFlag=false;
             kRed=0;
         }
-            
         deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
-        val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
+        if(getPress(btSel))
+            val=163;
+        else 
+            val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
         
         //一定時間通信してないと停止
         if(stopCounter>stopCount)
@@ -353,11 +361,15 @@
         do{
             if(!debugFlag)
                 break;
-            //pc.printf("val:%d\r\n",val);
+            //pc.printf("%d\r\n",DATA[6]);
+            //if(getPress(btSel))
+            //    pc.printf("up\r\n");
+            pc.printf("val:%d\r\n",val);
             //printf("%lf:%d:%d\r\n",deg,(int)DATA[3],(int)DATA[2]);
             //printf("%d\r\n",(int)DATA[0]);
             //pc.printf("DATA[1]%d\r\n",(int)(DATA[1]&0x02));
             //printf("deg:%lfX1:%d:bt%ddeg:%lfval:%d\r\n",deg,(int)DATA[1],getBt(btSqa),deg,val);
+            pc.printf("%lf\r\n",deg);
         }while(false);
     }
 }
\ No newline at end of file