Diff: main.cpp

Revision:

15:1c1ae4077900

Parent:

14:731c52eb333b

Child:

16:4b502070bea8

diff -r 731c52eb333b -r 1c1ae4077900 main.cpp
--- a/main.cpp	Mon Sep 08 07:22:30 2014 +0000
+++ b/main.cpp	Tue Sep 23 06:21:30 2014 +0000
@@ -7,7 +7,7 @@
  * motor(p17,p18):motor(p19,p20)
  *
  * Serial Pins
- * packet.arm[0]:arm  
+ * packet.arm[0]:arm
  * packet.leg[1]:omni
  * reset 0
  */
@@ -19,17 +19,27 @@
 #define XBEE_KEY 0xAA //keycode
 #define ARM_KEY 0xAA
 
-#define PWM 0.5
+#define PWM 1.0
+
+#define PWM_L 1.0
+#define PWM_R 1.0
+#define PWM_U 1.0
+#define PWM_D 1.0
 
-#define PWM_L 0.5
-#define PWM_R 0.5
-#define PWM_U 0.5
-#define PWM_D 0.5
+#define R 0x4
+#define L 0x1
+#define U 0x2
+#define D 0x8
+
+#define R_t 0x10
+#define L_t 0x40
+
+#define TIME 0
 
 BusOut check(LED1,LED2,LED3,LED4);
 BusOut motors(p13,p14,p15,p16,p17,p18,p19,p20);
 
-PwmOut pwm[4]={p21,p22,p23,p24}; 
+PwmOut pwm[4]= {p21,p22,p23,p24};
 
 Serial pc(USBTX,USBRX);
 
@@ -37,32 +47,53 @@
 MultiSerial armMbed(p9,p10,write);
 
 /* data structure */
-typedef struct{
+typedef struct {
 
     uint8_t arm[1];
     uint8_t leg;
 
-}xbee_packet;
+} xbee_packet;
 
 xbee_packet packet;
 
-uint8_t get_data[DATA_NUM]={0};
+uint8_t get_data[DATA_NUM]= {0};
 
 /* Out put PC stdout function */
+
 void pc_print(){
 
     static int count=0;
 
-    if(count==10e3){ 
+    if(count==10e3) {
         pc.printf(" arm = %d" ,packet.arm[0]);
         pc.printf(" leg = %d" ,packet.leg);
         count=0;
-    }   
+    }
     count++;
 }
 
+void stop_slowly(int count)
+{
+
+    for(float i=1.0; i>=0; i-=0.1) {
+        
+        /*for(int j=0; j>=4; j++) {
+
+            pwm[j] = 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
+    }
+    check = 0;
+}
+
 int main()
-{   
+{
 
     /* PWM init */
     pwm[0] = PWM;
@@ -74,50 +105,67 @@
 
     xbee.read_data(get_data,XBEE_KEY);
 
-    for(;;){
+    int counter=0;
 
-        wait_ms(0.1);  
+    for(;;) {
+
+        wait_ms(1.0);
 
         //memcpy(&packet, get_data, DATA_NUM);
 
         packet.arm[0] = get_data[1];
-        packet.leg = get_data[0];   
+        packet.leg = get_data[0];
 
-        check = get_data[1];
+        //check = get_data[1];
 
-        armMbed.write_data(pt_packet->arm,ARM_KEY);  
+        armMbed.write_data(pt_packet->arm,ARM_KEY);
 
         /* Stop */
-        if(packet.leg==0x0) motors = 0;
+        if(packet.leg==0x0) {
+
+            counter /= 10e2; //mirisec -->> sec
+
+            if(counter>=TIME) { //wait 2 sec
+
+                stop_slowly(counter);
+            }
 
-        /* PWM */     
-        if((packet.leg>>4)==0){
-            
+            motors = 0;
+            counter = 0; //cunter init
+
+        } else {
+
+            counter++;
+            check = 0xFF;
+        }
+
+        /* PWM */
+        if((packet.leg>>4)==0) {
+
             pwm[0] = PWM_L;
             pwm[1] = PWM_R;
             pwm[2] = PWM_U;
-            pwm[3] = PWM_D;                                
+            pwm[3] = PWM_D;
 
-        }else{
+        } else {
 
             pwm[0] = PWM;
             pwm[1] = PWM;
             pwm[2] = PWM;
             pwm[3] = PWM;
-                        
+
         }
 
         /* Direction of movement */
-        if(packet.leg&0x1) motors = 0x55; //01010101 L 接続に気をつけること
-        if(packet.leg&0x4) motors = 0xAA; //10101010 U 多分モータードライバの信号線をそれぞれ逆にすればOKかな
-        if(packet.leg&0x2) motors = 0x5A; //01011010 R      
-        if(packet.leg&0x8) motors = 0xA5; //10100101 D
+        if(packet.leg&R) motors = 0x55; //01010101 R 接続に気をつけること
+        if(packet.leg&L) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな
+        if(packet.leg&U) motors = 0x5A; //01011010 U
+        if(packet.leg&D) motors = 0xA5; //10100101 D
 
         /* Turn circling */
-        if(packet.leg&0x10) motors = 0x66; //01100110 
-        if(packet.leg&0x40) motors = 0x99; //10011001
+        if(packet.leg&R_t) motors = 0x66; //01100110
+        if(packet.leg&L_t) motors = 0x99; //10011001
 
         pc_print();
-
     }
 }
\ No newline at end of file