Diff: main.cpp

Revision:

25:a6e41d2efd0c

Parent:

23:54fddee73c88

Child:

26:9a7e9dd2746f

--- a/main.cpp	Fri Oct 03 10:45:48 2014 +0000
+++ b/main.cpp	Sun Oct 05 08:21:35 2014 +0000
@@ -19,20 +19,19 @@
 #define XBEE_KEY 0xAA //keycode
 #define ARM_KEY 0xAA
 
-#define PWM 0.5
+#define PWM 0.3
+#define PWM_SLOW 0.1
 
-#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_R 0x4
+#define R_L 0x1
+#define R_U 0x2
+#define R_D 0x8
 
-#define R_t 0x10
-#define L_t 0x40
+#define L_R 0x10
+#define L_L 0x40
+#define L_U 0x20 
+#define L_D 0x80
 
 #define TIME 0
 
@@ -152,6 +151,9 @@
     
     counter=0;
 
+    check = 0xFF;
+    wait(0.5);
+
     for(;;) {
 
         wait_ms(0.5);
@@ -161,35 +163,61 @@
         packet.leg = get_data[0];
 
         //check = get_data[1];
+            
+        check = packet.leg;
         
-        check = 0xFF;
-        if(packet.leg!=0){
-            
-            check = packet.leg;
-            }
 
         /* Stop */
     if(packet.leg==0x0) {
-            
+          
+          
+        motors = 0;
+          
+        pwm[0] = 0;
+        pwm[1] = 0;
+        pwm[2] = 0;
+        pwm[3] = 0;  
+        
+        /*
             for(;counter>=0;counter--){
                 
             //if(counter>=0){counter--;} //cunter init
             
-            move_slowly(counter);
-            wait_ms(0.1);
+            //move_slowly(counter);
+            //wait_ms(0.1);
             
-            }           
+            }
+            */           
 
         }else{
-
+            
+        /*
             if(counter<=10e3){
                 counter++;
             }
                 
-            move_slowly(counter);
+           move_slowly(counter);
+         */
+         
+         if((packet.leg==L_U) || (packet.leg==L_D)){
+            
+            pwm[0] = PWM_SLOW;
+            pwm[1] = PWM_SLOW;
+            pwm[2] = PWM_SLOW;
+            pwm[3] = PWM_SLOW;
+            
+            }else{
+                           
+            pwm[0] = PWM;
+            pwm[1] = PWM;
+            pwm[2] = PWM;
+            pwm[3] = PWM;
+           }
     }
 
+
         /* PWM */
+        
         /*if((packet.leg>>4)==0) {
 
             pwm[0] = PWM_L;
@@ -208,15 +236,17 @@
         */
 
         /* Direction of movement */
-        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
+        if(packet.leg&R_D) motors = 0x55; //01010101 R 接続に気をつけること
+        if(packet.leg&R_U) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな
+        if(packet.leg&R_R) motors = 0x5A; //01011010 U
+        if(packet.leg&R_L) motors = 0xA5; //10100101 D
 
         /* Turn circling */
-        if(packet.leg&R_t) motors = 0x66; //01100110
-        if(packet.leg&L_t) motors = 0x99; //10011001
-
+        if(packet.leg&L_L) motors = 0x66; //01100110
+        if(packet.leg&L_R) motors = 0x99; //10011001
+        //if(packet.leg&L_U) 
+        //if(packet.leg&L_D)
+            
         pc_print();
     }
 }
\ No newline at end of file