Diff: main.cpp

Revision:

39:18b1bc93a5f7

Parent:

38:098150180639

--- a/main.cpp	Tue Oct 14 12:24:42 2014 +0000
+++ b/main.cpp	Wed Oct 15 06:15:20 2014 +0000
@@ -66,7 +66,6 @@
             
             packet.arm[0] = 0; 
             packet.leg = 0;
-    
     }
 
 /*start display LED*/
@@ -97,15 +96,16 @@
 int main()
 {
         
+    int check_counter = 0;
+    uint8_t increase_counter = 0;
+    uint8_t counter_rx = 0;
+        
     /* PWM init */
     pwm[0] = PWM;
     pwm[1] = PWM;
     pwm[2] = PWM;
     pwm[3] = PWM;
     
-    /* Timer interrupt init */
-    xbee_check.attach(safety_mode,0.1);
-    
     /* stract data init */
     xbee_packet *pt_packet=&packet;
     
@@ -128,7 +128,7 @@
         /* Stop */
         if(packet.leg==0x0) {
 
-            motors = 0;
+            //motors = 0;
 
             pwm[0] = 0;
             pwm[1] = 0;
@@ -155,5 +155,26 @@
         //if(packet.leg&L_U)
         //if(packet.leg&L_D)
         
+        check_counter++; //abs counter
+        
+        if(xbee.re_counter_rx() > counter_rx) increase_counter++;
+        
+        counter_rx = xbee.re_counter_rx();
+        
+        if(check_counter >= 100){
+            
+            if(increase_counter <= 10){
+                 
+            safety_mode();
+
+            increase_counter = 0;          
+            check_counter = 0;
+            xbee.reset_counter_rx();
+
+            counter_rx = 0;
+            
+            }
+            
+        }
     }
 }
\ No newline at end of file