Diff: main.cpp

Revision:

19:cd036cc40a2d

Parent:

18:e3591b0b9e40

Child:

20:373fa71ce05b

--- a/main.cpp	Thu Oct 08 07:30:25 2015 +0000
+++ b/main.cpp	Tue Oct 13 07:40:34 2015 +0000
@@ -9,18 +9,17 @@
 #define btSph 5
 #define btCro 6
 #define btSqa 7
-#define btSel 0
-#define presUp 4
-#define presRight 5
-#define presDown 6
-#define presLeft 7
+#define presUp 0
+#define presRight 1
+#define presDown 2
+#define presLeft 3
 #define devideNum 32
 #define ED 132
 #define TD 144//208
 #define EL 134
 #define TL 200
 #define TNR 138//204
-#define TNL 150
+#define TNL 135
 #define debugFlag 1
 #define stopCount 33
 
@@ -30,8 +29,7 @@
 RawSerial Mechanum(D8,D2);//メカナム
 I2C i2c(D14,D15);//sda,scl
 //const int airAddr=0x20;
-const int airAddr=0xd0;
-const int airReg=10;
+const int airAddr=0xA0;
 const int valveAddr=0xC0;
 BusOut dI2c(PC_0,PC_1,PC_2,PC_3);//i2cデバッグ
 
@@ -77,7 +75,7 @@
     return atan2(-(y-128),x-128)/M_PI*180;//y軸反転、/PI*180でRadからDeg変換
 }
 
-int devidePoint(double y,double x,int threshold,float devide)
+double devidePoint(double y,double x,int threshold,float devide)
 {
     double tempX,tempY,dev;//仮のX,Yと分けた角度
     tempX=(x-128);tempY=(y-128);//それぞれ-128
@@ -135,60 +133,43 @@
     return val;
 }
 
-bool detectPole(int scVector)
+void detectPole(int scVector)
 {
     int vector=TD,laser=0;
-    bool btFlag=false,canFlag=false,breakFlag=false,stopFlag=false;
+    bool btFlag=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;Green=0.8;Red=0;
+            Blue=0.0f;Green=0.8f;Red=0;
             vector=scVector;
         }
         
         //片方のレーザーが反応したとき
-        if(btFlag&&!laser){
-            if(!(((int)DATA[1]&2)==2)){
+        if(btFlag){
+            if(!(int)DATA[1]&2){
                 laser=1;
-                Red=0.8;
                 vector=0;
             }
-            else if(!(((int)DATA[1]&1)==1)){
+            else if((int)DATA[1]&1){
                 laser=2;
-                Red=0.8;
                 vector=0;
             }
         }
         
         //もう片方のレーザーが反応した時
-        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;
+        else if(!(int)DATA[1]&laser&&laser){
+            Mechanum.putc(64);
+            break;
         }
         
         //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.0126f);
+        wait(0.001f);
     }
     btFlag=false;
     Blue=0;Green=0;Red=0;   
@@ -231,52 +212,93 @@
     double gValue=0,bValue=0,rValue=0;
     DigitalOut l(PB_2);
     int sqf=0;
-    double deg=32;
+    double deg;
     int val;
     i2c.frequency(400000); 
     setFCLED(0,0.8,0.8);
-    bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,detFinF=false;
+    bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,pUpF=false,pDownF=false,btLF=false,btRF=false,btTrF=false;
 
     while(true)
     {           
         //ポール検出モード
         if(getBt(btTri)&&getBt(btSph)){
-            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(deg<17)
+                detectPole(TNL);
+            else
+                detectPole(TNR);
+            //printf("DetectMode\r\n");
         }
         
         //射出
-        if(getBt(btL)&&obonFlag){
-            kRed=shotToDenziben(airReg,1,1,airAddr);
-            wait(0.5f);
-            kRed=shotToDenziben(airReg,0,1,airAddr);
+        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;
             kRed=0;
         }
         
         //射出
-        if(getBt(btR)&&obonFlag){
-            kRed=shotToDenziben(airReg,2,1,airAddr);
-            wait(0.5f);
-            kRed=shotToDenziben(airReg,0,1,airAddr);
-            //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
-            //i2c.write(airAddr,cmd,2);
+        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;
             kRed=0;
         }
         
-        //射出
-        if(getBt(btTri)&&obonFlag){
-            kRed=shotToDenziben(airReg,4,1,airAddr);
-            wait(0.5f);
-            kRed=shotToDenziben(airReg,0,1,airAddr);
+        //バルブ開放
+        if(getPress(presLeft)&&pLeftF){
+            pLeftF=false;
+            kRed=shotToDenziben(7,1,1,valveAddr);
+            wait(0.2);
+        }
+        else if(!getPress(presLeft)){
             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){
@@ -289,36 +311,38 @@
             
         //タッチセンサ
         if(((int)DATA[1]&12)==12){
-            //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;
+            bValue=0.0f;gValue=0.8f;rValue=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;
+        }
+        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 if(!getPress(presDown)){
+            pDownF=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);
-        }
-        else {
-            val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
-            //pc.printf("%lf\r\n",deg);
-        }
-        
+        val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
         
         //一定時間通信してないと停止
         if(stopCounter>stopCount)
@@ -331,15 +355,11 @@
         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