Diff: main.cpp

Revision:

27:68f76ea992bd

Parent:

26:9a7e9dd2746f

Child:

28:4da223ae4392

--- a/main.cpp	Tue Oct 07 08:01:29 2014 +0000
+++ b/main.cpp	Tue Oct 07 08:55:58 2014 +0000
@@ -56,8 +56,52 @@
 xbee_packet packet;
 
 uint8_t get_data[DATA_NUM]= {0};
+uint8_t check_data[2]={0};
 
-uint8_t check_data[2]={0};
+
+/*start display LED*/
+void display_LED(int kind){
+
+    switch(kind){
+        case 0:
+            check = 0xA;
+            wait(0.5);
+            check != 0xF;
+            wait(0.5);
+            
+            break;
+            
+        case 1:
+
+            check = 0xF;
+
+            break;
+        }
+    }
+
+
+/*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()
 {
@@ -71,6 +115,7 @@
     xbee_packet *pt_packet=&packet;
 
     //connect_check();
+    int radio_check=0;
     
     xbee.start_read();
     xbee.read_data(get_data,XBEE_KEY);
@@ -78,9 +123,12 @@
     armMbed.start_write();
     armMbed.write_data(pt_packet->arm,ARM_KEY);
     
-    check = 0xFF;
+    
+    
+    display_LED(1);
     wait(0.5);
-
+    if((packet.leg&L_U)==0){NVIC_SystemReset();} 
+    
     for(;;) {
 
         wait_ms(0.5);
@@ -91,9 +139,23 @@
 
         //check = get_data[1];
             
-        check = packet.leg;
+        check = packet.leg;   
+    
+    /*
+    if(xbee.readable_check() == 0) {
+ 
+            radio_check++;
+ 
+            if(radio_check>=500){                
+                safety_mode();
+            }
+
+    } else {
+ 
+            radio_check = 0;
+    }
+    */
         
-
         /* Stop */
     if(packet.leg==0x0) {
           
@@ -108,32 +170,12 @@
 
         }else{
                            
-            pwm[0] = PWM;
-            pwm[1] = PWM;
-            pwm[2] = PWM;
-            pwm[3] = PWM;
+        pwm[0] = PWM;
+        pwm[1] = PWM;
+        pwm[2] = PWM;
+        pwm[3] = PWM;
     }
 
-
-        /* PWM */
-        
-        /*if((packet.leg>>4)==0) {
-
-            pwm[0] = PWM_L;
-            pwm[1] = PWM_R;
-            pwm[2] = PWM_U;
-            pwm[3] = PWM_D;
-
-        } else {
-
-            pwm[0] = PWM;
-            pwm[1] = PWM;
-            pwm[2] = PWM;
-            pwm[3] = PWM;
-            
-        }
-        */
-
         /* Direction of movement */
         if(packet.leg&R_D) motors = 0x55; //01010101 R 接続に気をつけること
         if(packet.leg&R_U) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな