Diff: main.cpp

Revision:

18:e3591b0b9e40

Parent:

17:1942c2ba89a0

Child:

19:cd036cc40a2d

Child:

21:c5af214bfc05

--- a/main.cpp	Wed Oct 07 10:49:50 2015 +0000
+++ b/main.cpp	Thu Oct 08 07:30:25 2015 +0000
@@ -30,7 +30,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デバッグ
 
@@ -76,7 +77,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
@@ -134,15 +135,15 @@
     return val;
 }
 
-void detectPole(int scVector)
+bool detectPole(int scVector)
 {
     int vector=TD,laser=0;
-    bool btFlag=false,canFlag=false,breakFlag=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)
-            break;
+            return breakFlag;
         else if(!(getBt(btTri)&&getBt(btSph)))
             canFlag=true;
             
@@ -170,10 +171,15 @@
         }
         
         //もう片方のレーザーが反応した時
-        else if(!(((int)DATA[1]&laser)==laser)&&laser){
+        else if(!(((int)DATA[1]&laser)==laser)&&laser&&!stopFlag){
             Green=0;Red=0.8;Blue=0.8;
-            wait(0.0125f);
-            Mechanum.putc(64);
+            vector=64;
+            stopFlag=true;
+            wait(0.05);
+        }
+        //ストップ後
+        else if(stopFlag)
+        {
             vector=162;
             breakFlag=true;
         }
@@ -229,7 +235,7 @@
     int val;
     i2c.frequency(400000); 
     setFCLED(0,0.8,0.8);
-    bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false;
+    bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,detFinF=false;
 
     while(true)
     {           
@@ -238,25 +244,27 @@
             if(deg>devideNum-1);
             else if(deg<17){
                 //pc.printf("R\r\n");
-                detectPole(TNR);
+                detFinF=detectPole(TNR);
             }
             else{
                 //pc.printf("L\r\n");
-                detectPole(TNL);
+                detFinF=detectPole(TNL);
             }
         }
         
         //射出
         if(getBt(btL)&&obonFlag){
-            kRed=shotToDenziben(0x02,39,1,airAddr);
-            wait(0.1f);
+            kRed=shotToDenziben(airReg,1,1,airAddr);
+            wait(0.5f);
+            kRed=shotToDenziben(airReg,0,1,airAddr);
             kRed=0;
         }
         
         //射出
         if(getBt(btR)&&obonFlag){
-            kRed=shotToDenziben(0x02,23,1,airAddr);
-            wait(0.1f);
+            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);
             kRed=0;
@@ -264,52 +272,11 @@
         
         //射出
         if(getBt(btTri)&&obonFlag){
-            kRed=shotToDenziben(0x02,15,1,airAddr);
-            wait(0.2f);
-            kRed=shotToDenziben(0x02,0,1,airAddr);
+            kRed=shotToDenziben(airReg,4,1,airAddr);
+            wait(0.5f);
+            kRed=shotToDenziben(airReg,0,1,airAddr);
             kRed=0;
         }
-        
-        //バルブ開放
-        if(getPress(presLeft)&&pLeftF){
-            pLeftF=false;
-            kRed=shotToDenziben(7,1,1,valveAddr);
-            wait(2.0);
-            if(!kRed)
-                kRed=shotToDenziben(7,0,1,valveAddr);
-        }
-        else if(!getPress(presLeft)){
-            kRed=0;
-            pLeftF=true;
-        }
-            
-        //バルブ開放
-        if(getPress(presRight)&&pRightF){
-            pRightF=false;
-            kRed=shotToDenziben(8,1,1,valveAddr);
-            wait(2.0);
-            if(!kRed)
-                kRed=shotToDenziben(8,0,1,valveAddr);
-        }
-        else if(!getPress(presRight)){
-            kRed=0;
-            pRightF=true;
-        }
-        
-        //バルブ開放
-        if(getBt(btCro)&&btCrF){
-            btCrF=false;
-            kRed=shotToDenziben(9,1,1,valveAddr);
-            wait(0.2);
-            if(!kRed)
-                kRed=shotToDenziben(9,0,1,valveAddr);
-        }
-        else if(!getBt(btCro))
-        {
-            kRed=0;
-            btCrF=true;
-        }
-
 
         //レーザーポインタ
         if(getBt(btSqa)&&sqf){
@@ -322,33 +289,36 @@
             
         //タッチセンサ
         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;
-        }
         
         //お盆回し
         if(getPress(presUp)){
-            kRed=shotToDenziben(0x02,7,1,airAddr);
-            wait(0.1);
             obonFlag=true;
             rValue=0.8;gValue=0;bValue=0.8;
             kRed=0;
         }
         else if(getPress(presDown)){
-            kRed=shotToDenziben(0x02,0,1,airAddr);
-            wait(0.1);
             bValue=0.8f;gValue=0;rValue=0;
             obonFlag=false;
             kRed=0;
         }
+        else{
+            bValue=0.8f;gValue=0;rValue=0;
+        }
+            
+        
         deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
-        if(getPress(btSel))
-            val=163;
-        else 
+        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);
+        }
+        
         
         //一定時間通信してないと停止
         if(stopCounter>stopCount)
@@ -364,12 +334,12 @@
             //pc.printf("%d\r\n",DATA[6]);
             //if(getPress(btSel))
             //    pc.printf("up\r\n");
-            pc.printf("val:%d\r\n",val);
+            //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);
+            //pc.printf("%lf\r\n",deg);
         }while(false);
     }
 }
\ No newline at end of file