Diff: main.cpp

Revision:

35:78f5eec4f36c

Parent:

34:aa464f72d111

Child:

36:bacc260185d6

diff -r aa464f72d111 -r 78f5eec4f36c main.cpp
--- a/main.cpp	Sun Oct 12 00:51:08 2014 +0000
+++ b/main.cpp	Tue Oct 14 09:00:24 2014 +0000
@@ -1,4 +1,4 @@
-/*
+/*S
  * 4WD_OMNI_simple  by Tomoki Hokida
  * Simple Type
  *
@@ -59,6 +59,21 @@
 uint8_t get_data[DATA_NUM]= {0};
 uint8_t check_data[2]= {0};
 
+void safety_mode(){
+
+            pwm[0] = 0;
+            pwm[1] = 0;
+            pwm[2] = 0;
+            pwm[3] = 0;
+            
+            get_data[0] = 0;
+            get_data[1] = 0;
+            
+            packet.arm[0] = 0; 
+            packet.leg = 0;
+    
+    }
+
 /*start display LED*/
 void display_LED(int kind)
 {
@@ -78,6 +93,7 @@
             check = 0xF;
             wait(0.5);
             check = 0;
+            wait(0.5);
             break;
     }
 }
@@ -91,64 +107,48 @@
     pwm[2] = PWM;
     pwm[3] = PWM;
 
+    /* Timer init*/
+    xbee_check.start();
+    
+    /* stract data init */
     xbee_packet *pt_packet=&packet;
-
-    //connect_check();
-    int abs_count=0;
-    int radio_count=0;
+    
+    /* Serial connection init */
     
-    xbee.start_read();
-    xbee.read_data(get_data,XBEE_KEY);
+        xbee.start_read();
+        xbee.read_data(get_data,XBEE_KEY);
 
-    armMbed.start_write();
-    armMbed.write_data(pt_packet->arm,ARM_KEY);
+        armMbed.start_write();
+        armMbed.write_data(pt_packet->arm,ARM_KEY);
 
-    display_LED(1);
-
-    //int counter=0;
+        display_LED(1);
+        
+        if(xbee.check_rx()==0){
+        
+            NVIC_SystemReset();    
+            
+            }
     
-   // int pwm_check=0;
+        
+        // NVIC_DisableIRQ(UART1_IRQn);
+        // NVIC_DisableIRQ(UART0_IRQn);
 
     for(;;) {
-
-        //wait_ms(0.5);
-
-        //memcpy(&packet, get_data, DATA_NUM);
         
         xbee_check.reset();
         xbee_check.start();
-        for(;xbee_check.read_ms()<=5;){
-
-            abs_count++;            
-
-            if(xbee.readable_check() == 0) {
-          
-            radio_count++; 
-            }
-        }
+        
+               
+        if(xbee_check.read()>=1)
+        { 
         
-        if(abs_count==radio_count){
-            abs_count=0;
-                
-            motors = 0;
-            
-            pwm[0] = 0;
-            pwm[1] = 0;
-            pwm[2] = 0;
-            pwm[3] = 0;
-            
-            check =0x3;
-            
-            get_data[0] = 0;
-            get_data[1] = 0;
-            
-            packet.arm[0] = 0; //できれば構造体を直接初期化
-            packet.leg = 0;
-            
-            radio_count=0;
-            
+        safety_mode();
+        
+        xbee_check.stop();
+        xbee_check.reset();
+        xbee_check.start();
+        
         }
-        
         packet.arm[0] = get_data[1];
         packet.leg = get_data[0];