Diff: main.cpp

Revision:

34:aa464f72d111

Parent:

33:9c98ab97084e

Child:

35:78f5eec4f36c

--- a/main.cpp	Sat Oct 11 05:05:12 2014 +0000
+++ b/main.cpp	Sun Oct 12 00:51:08 2014 +0000
@@ -22,7 +22,6 @@
 #define PWM 0.5
 #define PWM_SLOW 0.1
 
-
 #define R_R 0x4
 #define R_L 0x1
 #define R_U 0x2
@@ -45,6 +44,8 @@
 MultiSerial xbee(p28,p27);
 MultiSerial armMbed(p9,p10);
 
+Timer xbee_check;
+
 /* data structure */
 typedef struct {
 
@@ -58,7 +59,6 @@
 uint8_t get_data[DATA_NUM]= {0};
 uint8_t check_data[2]= {0};
 
-
 /*start display LED*/
 void display_LED(int kind)
 {
@@ -82,29 +82,6 @@
     }
 }
 
-
-/*Safety Mode*/
-void safety_mode()
-{
-    xbee.stop_read();
-    armMbed.stop_write();
-
-    for(;;) {
-
-        motors = 0;
-        display_LED(0);
-
-        if(xbee.readable_check()==1) {
-
-            NVIC_SystemReset();
-
-            //xbee.start_read();
-            //arm_mbed.start_write();
-            return;
-        }
-    }
-}
-
 int main()
 {
 
@@ -137,21 +114,21 @@
         //wait_ms(0.5);
 
         //memcpy(&packet, get_data, DATA_NUM);
-        packet.arm[0] = get_data[1];
-        packet.leg = get_data[0];
+        
+        xbee_check.reset();
+        xbee_check.start();
+        for(;xbee_check.read_ms()<=5;){
 
-        abs_count++;            
+            abs_count++;            
 
-        if(xbee.readable_check() == 0) {
+            if(xbee.readable_check() == 0) {
           
             radio_count++; 
-                  
+            }
         }
         
-        if(abs_count>=10){
+        if(abs_count==radio_count){
             abs_count=0;
-   
-            if(radio_count>=5){
                 
             motors = 0;
             
@@ -162,14 +139,18 @@
             
             check =0x3;
             
+            get_data[0] = 0;
+            get_data[1] = 0;
+            
             packet.arm[0] = 0; //できれば構造体を直接初期化
             packet.leg = 0;
             
-            }
-            
             radio_count=0;
             
-        } 
+        }
+        
+        packet.arm[0] = get_data[1];
+        packet.leg = get_data[0]; 
 
         /* Stop */
         if(packet.leg==0x0) {