Diff: main.cpp

Revision:

5:e1b1b9a43ccb

Parent:

1:a961c533bc91

diff -r a4c8c442bb7b -r e1b1b9a43ccb main.cpp
--- a/main.cpp	Thu May 08 13:13:14 2014 +0000
+++ b/main.cpp	Sun May 11 09:08:50 2014 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "XBee.h"
 #define BUFFER_SIZE 512
 
 //pin set
@@ -6,16 +7,18 @@
 I2C i2c(p9, p10);        //sda, scl,MFU
 Serial pc(USBTX, USBRX); //tx, rx
 Serial gps(p28, p27);    //tx, rx
-Serial xbee(p13,p14);    //tx,rx
+XBee xbee(p13,p14);    //tx,rx
+ZBRxResponse zbRx = ZBRxResponse();
+
 
 //motor
-DigitalOut motor1_A(p21);
-DigitalOut motor1_B(p22);
-PwmOut motor1_pwm(p23);
+DigitalOut motorL_A(p21);
+DigitalOut motorL_B(p22);
+PwmOut motorL_pwm(p23);
 
-DigitalOut motor2_A(p24);
-DigitalOut motor2_B(p25);
-PwmOut motor2_pwm(p26);
+DigitalOut motorR_A(p24);
+DigitalOut motorR_B(p25);
+PwmOut motorR_pwm(p26);
 
 //encorder
 InterruptIn encoder1_A(p15);
@@ -124,6 +127,7 @@
     //encorder_count = 0;
     
     //Output GPS data -> xbee
+    /*
     char val;
     gps_buf.rp = gps_buf.last_head_rp; 
     while(1){
@@ -131,7 +135,7 @@
         xbee.printf("%c",val);
         if(val == 0x0a) break;   
         }
-        
+      */  
     }
     
 void gps_rx(){
@@ -143,6 +147,7 @@
         }    
     }
     
+    /*
 void xbee_rx(){
     char val = xbee.getc();
     if(val == 'a') {
@@ -168,6 +173,51 @@
         
     
     }
+*/
+
+void change_speed(unsigned char L_state,unsigned char L_pwm,unsigned char R_state,unsigned char R_pwm){
+    
+    switch(L_state){
+        case 0:
+            motorL_A = 0;
+            motorL_B = 0;
+        
+        case 1:
+            motorL_A = 1;
+            motorL_B = 0;
+        
+        case 2:
+            motorL_A = 0;
+            motorL_B = 1;        
+        }
+    
+    motorL_pwm = (float)L_pwm/256.0;
+    
+    
+    
+    switch(L_state){
+        case 0:
+            motorR_A = 0;
+            motorR_B = 0;
+        
+        case 1:
+            motorR_A = 1;
+            motorR_B = 0;
+        
+        case 2:
+            motorR_A = 0;
+            motorR_B = 1;        
+        }
+    
+    motorR_pwm = (float)R_pwm/256.0;
+    
+    
+    
+    
+    
+    
+    
+    }
 
 void send_GPS_command(){
     char gps_command1[] = "$PMTK220,500*2B";
@@ -195,13 +245,13 @@
     //set pc frequency to 57600bps 
     pc.baud(57600); 
     //set xbee frequency to 57600bps
-    xbee.baud(57600);    
+    xbee.begin(57600);    
     
     //GPS setting
     send_GPS_command();
     init_ring();
     gps.attach(gps_rx,Serial::RxIrq);
-    xbee.attach(xbee_rx,Serial::RxIrq);
+    //xbee.attach(xbee_rx,Serial::RxIrq);
     wait(5);
 
     //Encorder Innterrapt Setting
@@ -222,8 +272,34 @@
     
     //interrupt start
     axis.attach(&axisRenovation, 0.1);
-    motor1_pwm = 0.0;
+    motorL_pwm = 0.0;
+    motorR_pwm = 0.0;
+    
     
     while (1) {
+        
+        
+        xbee.readPacket();
+            if (xbee.getResponse().isAvailable()) {
+                if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
+                xbee.getResponse().getZBRxResponse(zbRx);
+                unsigned char *buf = zbRx.getFrameData(); 
+                
+                /*for(int i = 0 ;i < zbRx.getPacketLength(); i++){
+                    pc.printf("%d\n",buf[i]);
+                    }
+                    */
+                      //pc.printf("%c%c%c\n",buf[11],buf[12],buf[13]);
+                      change_speed(buf[17],buf[18],buf[20],buf[21]);
+                      
+                      
+            }
+    
+            
+            
+        }
+     
+        
     }
+    
 }
\ No newline at end of file