Diff: main.cpp

Revision:

22:bfeb1c6e2539

Parent:

20:a322730ea5d7

Child:

23:54fddee73c88

--- a/main.cpp	Sun Sep 28 02:01:51 2014 +0000
+++ b/main.cpp	Wed Oct 01 09:18:03 2014 +0000
@@ -60,6 +60,8 @@
 
 uint8_t check_data[2]={0};
 
+int counter=0;
+
 
 /* Out put PC stdout function */
 void pc_print()
@@ -114,23 +116,18 @@
     xbee.stop_write();
 }
 
-void stop_slowly(int count)
-{
+void move_slowly(int count)
+{   
 
-    for(float i=1.0; i>=0; i-=0.1) {
-
-        /*for(int j=0; j>=4; j++) {
-            pwm[j] = i;
+        float i=(count/10e3)*PWM; //mirisec -->> sec
 
-        }*/
+        pwm[0] = i;
+        pwm[1] = i;
+        pwm[2] = i;
+        pwm[3] = i;
 
-        pwm[0] = 0.1;
-        pwm[1] = 0.1;
-        pwm[2] = 0.1;
-        pwm[3] = 0.1;
-
-        wait(count*10e-3); //count value * 0.01 sec
-    }
+        wait(count*2*10e-3); //count value * 0.01 sec
+    
     //check = 0;
 }
 
@@ -149,12 +146,11 @@
     
     xbee.start_read();
     xbee.read_data(get_data,XBEE_KEY);
-    
-    
+        
     armMbed.start_write();
     armMbed.write_data(pt_packet->arm,ARM_KEY);
     
-    int counter=0;
+    counter=0;
 
     for(;;) {
 
@@ -168,22 +164,23 @@
         
 
         /* Stop */
-        if(packet.leg==0x0) {
-
-            counter /= 10e2; //mirisec -->> sec
+    if(packet.leg==0x0) {
 
-            if(counter>=TIME) { //wait 2 sec
+            if(counter!=0){
+                
+                move_slowly(counter);
+                
+                counter--;
+            } //cunter init
 
-                stop_slowly(counter);
-            }
 
             motors = 0;
-            counter = 0; //cunter init
+            
 
         } else {
 
-            counter++;
-        }
+            if(counter<=10e3){counter++;}
+    }
 
         /* PWM */
         if((packet.leg>>4)==0) {
@@ -199,7 +196,7 @@
             pwm[1] = PWM;
             pwm[2] = PWM;
             pwm[3] = PWM;
-
+            
         }
 
         /* Direction of movement */
@@ -209,8 +206,8 @@
         if(packet.leg&D) motors = 0xA5; //10100101 D
 
         /* Turn circling */
-        if(packet.leg&L_t) motors = 0x66; //01100110
-        if(packet.leg&R_t) motors = 0x99; //10011001
+        if(packet.leg&R_t) motors = 0x66; //01100110
+        if(packet.leg&L_t) motors = 0x99; //10011001
 
         pc_print();
     }