Diff: main.cpp

Revision:

3:26ae0e32030d

Parent:

2:7cd9f9cc2376

Child:

4:1fc3f9627302

--- a/main.cpp	Sun Oct 05 01:00:24 2014 +0000
+++ b/main.cpp	Sun Oct 05 09:49:57 2014 +0000
@@ -1,5 +1,6 @@
 /*
- * receibes_robot_wheel  by Tomoki Hokida
+ * 4WD_OMNI_simple  by Tomoki Hokida
+ * Simple Type
  *
  * motor pins piar
  * motor(p13,p14):motor(p15,p16)
@@ -18,26 +19,30 @@
 #define XBEE_KEY 0xAA //keycode
 #define ARM_KEY 0xAA
 
-#define PWM 0.5
+#define PWM 0.3
+#define PWM_SLOW 0.1
 
-#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
 
 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);
 
 serial_extend xbee(p28,p27);
-serial_extend arm_mbed(p9,p10);
+serial_extend armMbed(p9,p10);
 
 /* data structure */
 typedef struct {
@@ -50,115 +55,91 @@
 xbee_packet packet;
 
 uint8_t get_data[DATA_NUM]= {0};
-uint8_t check_data[2]= {0};
-
-/* Out put PC stdout function */
-void pc_print()
-{
-    static int count=0;
-
-    if(count==10e3) {
-        pc.printf(" arm = %d" ,packet.arm[0]);
-        pc.printf(" leg = %d" ,packet.leg);
-        count=0;
-    }
-    count++;
-}
 
-/*Safety Mode*/
-void safety_mode()
-{
-    xbee.stop_read();
-    arm_mbed.stop_write();
-
-    for(;;) {
-
-        motors = 0;
+uint8_t check_data[2]={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 counter=0;
 
 int main()
 {
 
+    /* PWM init */
+    pwm[0] = PWM;
+    pwm[1] = PWM;
+    pwm[2] = PWM;
+    pwm[3] = PWM;
+
     xbee_packet *pt_packet=&packet;
 
-    /*Reception Start*/
+    //connect_check();
+    
     xbee.start_read();
     xbee.read_data(get_data,XBEE_KEY);
+        
+    armMbed.start_write();
+    armMbed.write_data(pt_packet->arm,ARM_KEY);
     
-    /*Send Start*/
-    arm_mbed.start_write();
-    arm_mbed.write_data(pt_packet->arm,ARM_KEY);
-    
+    counter=0;
+
     check = 0xFF;
-
-    //int radio_check=0;
+    wait(0.5);
 
     for(;;) {
 
-        /*PWM init*/
-
-        pwm[0]=PWM;
-        pwm[1]=PWM;
-        pwm[2]=PWM;
-        pwm[3]=PWM;
-
-/*
-        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];
 
         //check = get_data[1];
-        
+            
         check = packet.leg;
-
+        
 
         /* Stop */
-        if(packet.leg==0x0) {
+    if(packet.leg==0x0) {
+          
+          
+        motors = 0;
+          
+        pwm[0] = 0;
+        pwm[1] = 0;
+        pwm[2] = 0;
+        pwm[3] = 0;  
+        
+       
 
-            motors = 0x00;
-        }
+        }else{
+            
+         
+         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;
+           }
+    }
 
         /* 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
-
-        pc_print();
+        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)
+            
     }
 }
\ No newline at end of file