Diff: main.cpp

Revision:

11:1ca2498815c9

Parent:

9:489a085d1703

Child:

12:6445a3c3d34c

--- a/main.cpp	Thu Sep 17 04:58:57 2015 +0000
+++ b/main.cpp	Sat Sep 19 02:11:33 2015 +0000
@@ -20,6 +20,8 @@
 #define TL 200
 #define TNR 138//204
 #define TNL 135
+#define debugFlag 1
+#define stopCount 33
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 //Serial Dev(D8,D2);//コントローラー
@@ -40,6 +42,7 @@
 char DATA[N];
 void rotate(double setAngle);
 void speed();
+int stopCounter=0;
 
 void num()//割り込まれたら7回受信
 {
@@ -48,10 +51,9 @@
         for(int i = 0 ;i<N ;i++ )
         {
             DATA[i]=Dev.getc();
-            //printf("%d",DATA[i]);
         }
     }
-   //printf("\r\n");
+    stopCounter=0;
 }
 
 bool getBt(int num)
@@ -147,6 +149,8 @@
             Blue=0.0f;Green=0.8f;Red=0;
             vector=64;
         }
+        
+        //方向選択
         else if((int)deg==devideNum)
         {
             deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
@@ -194,17 +198,11 @@
     pc.baud(230400);
     Dev.baud(921600);
     Mechanum.baud(230400);
-    char shotL[]={57,0};
-    char shotR[]={58,0};
-    char shotT[]={60,0};
-    char obon[]={56,0};
-    char read[2]={10,0};
     DigitalIn bt(USER_BUTTON);
     Dev.attach(num,Serial::RxIrq);//受信割り込み設定
     Green=0;Blue=0;Red=0;
     DigitalOut l(PB_2);
     int sqf=0;
-    //pc.printf("Hello World !\n");
     double deg;
     int val;
     i2c.frequency(400000); 
@@ -297,16 +295,24 @@
             kRed=0;
         }
             
-        //deg=pointToDeg((double)DATA[3],(double)DATA[2],30);
         deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
-        //printf("%lf:%d:%d\r\n",deg,(int)DATA[3],(int)DATA[2]);
-        //printf("%d\r\n",(int)DATA[0]);
         val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
-        //printf("val:%d\r\n",val);
+        
+        //一定時間通信してないと停止
+        if(stopCounter>stopCount)
+            val=64;
+        else
+            stopCounter++;
         Mechanum.putc(val);
         wait(0.03f);
-        
-        //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);
+        do{
+            if(!debugFlag)
+                break;
+            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);
+        }while(false);
     }
 }
\ No newline at end of file