Diff: main.cpp

Revision:

24:37a17b7c6201

Parent:

23:54fddee73c88

--- a/main.cpp	Fri Oct 03 10:45:48 2014 +0000
+++ b/main.cpp	Sat Oct 04 09:16:38 2014 +0000
@@ -38,13 +38,14 @@
 
 BusOut check(LED1,LED2,LED3,LED4);
 BusOut motors(p13,p14,p15,p16,p17,p18,p19,p20);
-
 PwmOut pwm[4]= {p21,p22,p23,p24};
 
 Serial pc(USBTX,USBRX);
 
 MultiSerial xbee(p28,p27);
-MultiSerial armMbed(p9,p10);
+MultiSerial arm_mbed(p9,p10);
+
+
 
 /* data structure */
 typedef struct {
@@ -84,8 +85,9 @@
         
         if(xbee.readable_check()==1){check++;}
         
-        wait_ms(1.0);
+        wait_ms(1.0);        
         }
+        
     return check;
     }
 
@@ -131,14 +133,34 @@
     //check = 0;
 }
 
+void safety_mode()
+{
+    xbee.stop_read();
+    arm_mbed.stop_write();
+    
+    for(;;) {
+
+        motors = 0;
+
+        check = 0xA;
+        wait(0.5);
+        check != 0xF;
+        wait(0.5);
+
+        if(xbee.readable_check()==1) {
+
+            xbee.start_read();
+            arm_mbed.start_write();
+            return;
+
+        }
+    }
+}
+
 int main()
 {
 
-    /* PWM init */
-    pwm[0] = PWM;
-    pwm[1] = PWM;
-    pwm[2] = PWM;
-    pwm[3] = PWM;
+
 
     xbee_packet *pt_packet=&packet;
 
@@ -147,16 +169,31 @@
     xbee.start_read();
     xbee.read_data(get_data,XBEE_KEY);
         
-    armMbed.start_write();
-    armMbed.write_data(pt_packet->arm,ARM_KEY);
+    arm_mbed.start_write();
+    arm_mbed.write_data(pt_packet->arm,ARM_KEY);
     
     counter=0;
+    
+    int radio_check=0;
 
     for(;;) {
+        
+        if(xbee.readable_check() == 0) {
+            
+            radio_check++;
 
+            if(radio_check>=500) {
+                safety_mode();
+            }
+        }else{
+            
+        radio_check = 0;
+        
+        }
+        
         wait_ms(0.5);
 
-        //memcpy(&packet, get_data, DATA_NUM);
+
         packet.arm[0] = get_data[1];
         packet.leg = get_data[0];
 
@@ -189,24 +226,6 @@
             move_slowly(counter);
     }
 
-        /* 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) motors = 0x55; //01010101 R 接続に気をつけること
         if(packet.leg&L) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな