Diff: main.cpp

Revision:

20:373fa71ce05b

Parent:

19:cd036cc40a2d

Child:

22:ab619079fb8d

--- a/main.cpp	Tue Oct 13 07:40:34 2015 +0000
+++ b/main.cpp	Fri Oct 16 09:32:02 2015 +0000
@@ -9,19 +9,21 @@
 #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
+#define shotWait 0.2
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 //Serial Dev(D8,D2);//コントローラー
@@ -29,7 +31,8 @@
 RawSerial Mechanum(D8,D2);//メカナム
 I2C i2c(D14,D15);//sda,scl
 //const int airAddr=0x20;
-const int airAddr=0xA0;
+const int airAddr=0xd0;
+const int airReg=10;
 const int valveAddr=0xC0;
 BusOut dI2c(PC_0,PC_1,PC_2,PC_3);//i2cデバッグ
 
@@ -75,7 +78,7 @@
     return atan2(-(y-128),x-128)/M_PI*180;//y軸反転、/PI*180でRadからDeg変換
 }
 
-double devidePoint(double y,double x,int threshold,float devide)
+int devidePoint(double y,double x,int threshold,float devide)
 {
     double tempX,tempY,dev;//仮のX,Yと分けた角度
     tempX=(x-128);tempY=(y-128);//それぞれ-128
@@ -133,43 +136,60 @@
     return val;
 }
 
-void detectPole(int scVector)
+bool detectPole(int scVector)
 {
     int vector=TD,laser=0;
-    bool btFlag=false;
+    bool btFlag=false,canFlag=false,breakFlag=false,stopFlag=false;
     Blue=0.0f;Green=0.0f,Red=0.8f;
     while(true)
-    {
+    {    
+        if((getBt(btTri)&&getBt(btSph)&&canFlag)||breakFlag)
+            return breakFlag;
+        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;
         }
         
         //片方のレーザーが反応したとき
-        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){
-            Mechanum.putc(64);
-            break;
+        else if(!(((int)DATA[1]&laser)==laser)&&laser&&!stopFlag){
+            Green=0;Red=0.8;Blue=0.8;
+            vector=64;
+            stopFlag=true;
+            wait(0.05);
+        }
+        //ストップ後
+        else if(stopFlag)
+        {
+            vector=162;
+            breakFlag=true;
         }
         
         //pc.printf("%d\r\n",DATA[1]);
+        //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;   
@@ -212,93 +232,52 @@
     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);
-    bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,pUpF=false,pDownF=false,btLF=false,btRF=false,btTrF=false;
+    bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,detFinF=false;
 
     while(true)
     {           
         //ポール検出モード
         if(getBt(btTri)&&getBt(btSph)){
-            if(deg<17)
-                detectPole(TNL);
-            else
-                detectPole(TNR);
-            //printf("DetectMode\r\n");
+            if(deg>devideNum-1);
+            else if(deg<17){
+                //pc.printf("R\r\n");
+                detFinF=detectPole(TNR);
+            }
+            else{
+                //pc.printf("L\r\n");
+                detFinF=detectPole(TNL);
+            }
         }
         
         //射出
-        if(getBt(btL)&&obonFlag&&btLF){
-            kRed=shotToDenziben(0x02,39,1,airAddr);
-            wait(0.1f);
-        }
-        else if(!getBt(btL)){
-            btLF=true;
-            kRed=0;
-        }
-            
-        
-        //射出
-        if(getBt(btR)&&obonFlag&&btRF){
-            kRed=shotToDenziben(0x02,23,1,airAddr);
-            wait(0.1f);
-            //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
-            //i2c.write(airAddr,cmd,2);
-        }
-        else if(!getBt(btR)){
-            btRF=true;
+        if(getBt(btL)&&obonFlag){
+            kRed=shotToDenziben(airReg,1,1,airAddr);
+            wait(shotWait);
+            kRed=shotToDenziben(airReg,0,1,airAddr);
             kRed=0;
         }
         
         //射出
-        if(getBt(btTri)&&obonFlag&&btTrF){
-            kRed=shotToDenziben(0x02,15,1,airAddr);
-            wait(0.2f);
-            kRed=shotToDenziben(0x02,0,1,airAddr);
-            kRed=0;
-        }
-        else if(!getBt(btTri)){
-            btTrF=true;
+        if(getBt(btR)&&obonFlag){
+            kRed=shotToDenziben(airReg,2,1,airAddr);
+            wait(shotWait);
+            kRed=shotToDenziben(airReg,0,1,airAddr);
+            //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
+            //i2c.write(airAddr,cmd,2);
             kRed=0;
         }
         
-        //バルブ開放
-        if(getPress(presLeft)&&pLeftF){
-            pLeftF=false;
-            kRed=shotToDenziben(7,1,1,valveAddr);
-            wait(0.2);
-        }
-        else if(!getPress(presLeft)){
+        //射出
+        if(getBt(btTri)&&obonFlag){
+            kRed=shotToDenziben(airReg,4,1,airAddr);
+            wait(shotWait);
+            kRed=shotToDenziben(airReg,0,1,airAddr);
             kRed=0;
-            pLeftF=true;
-        }
-            
-        //バルブ開放
-        if(getPress(presRight)&&pRightF){
-            pRightF=false;
-            kRed=shotToDenziben(8,1,1,valveAddr);
-            wait(0.2);
         }
-        else if(!getPress(presRight)){
-            kRed=0;
-            pRightF=true;
-        }
-        
-        //バルブ開放
-        if(getBt(btCro)&&btCrF){
-            btCrF=false;
-            kRed=shotToDenziben(9,1,1,valveAddr);
-            wait(0.2);
-        }
-        else if(!getBt(btCro))
-        {
-            kRed=0;
-            btCrF=true;
-        }
-        
-
 
         //レーザーポインタ
         if(getBt(btSqa)&&sqf){
@@ -309,40 +288,38 @@
         else if(!getBt(btSqa))
             sqf=1;
             
-        //タッチセンサ
-        if(((int)DATA[1]&12)==12){
-            bValue=0.0f;gValue=0.8f;rValue=0;
+        //レーザーセンサ
+        if(!((int)DATA[1]&2)==2&&!((int)DATA[1]%1)==1){
+            //Mechanum.putc(162);
+        }
+        
+        //お盆回し
+        if(getPress(presUp)){
+            obonFlag=true;
+            rValue=0.8;gValue=0;bValue=0.8;
+            kRed=0;
+        }
+        else if(getPress(presDown)){
+            bValue=0.8f;gValue=0;rValue=0;
+            obonFlag=false;
+            kRed=0;
         }
         else{
             bValue=0.8f;gValue=0;rValue=0;
         }
+            
         
-        //お盆回し
-        if(getPress(presUp)&&pUpF){
-            kRed=shotToDenziben(0x02,7,1,airAddr);
-            wait(0.2);
-            obonFlag=true;
-            pUpF=false;
-            rValue=0.8;gValue=0;bValue=0.8;
-        }
-        else if(!getPress(presUp)){
-            pUpF=true;
-            kRed=0;
+        deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
+        if(getPress(btSel)||(detFinF&&deg!=devideNum)){
+            detFinF=false;
+            Mechanum.putc(163);
+            wait(0.1);
         }
-        if(getPress(presDown)&&pDownF&&obonFlag){
-            kRed=shotToDenziben(0x02,0,1,airAddr);
-            wait(0.2);
-            bValue=0.8f;gValue=0;rValue=0;
-            pDownF=false;
-            obonFlag=false;
+        else {
+            val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
+            //pc.printf("%lf\r\n",deg);
         }
-        else if(!getPress(presDown)){
-            pDownF=true;
-            kRed=0;
-        }
-            
-        deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
-        val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
+        
         
         //一定時間通信してないと停止
         if(stopCounter>stopCount)
@@ -355,11 +332,15 @@
         do{
             if(!debugFlag)
                 break;
+            //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