Diff: main.cpp

Revision:

1:a5f98c7e1feb

Parent:

0:d8f3aa3b6876

Child:

2:e9a8664b52ff

--- a/main.cpp	Sat Apr 11 11:57:26 2015 +0000
+++ b/main.cpp	Sat Apr 11 12:25:34 2015 +0000
@@ -16,6 +16,7 @@
 #include "AigamozuControlPackets.h"
 #include "agzIDLIST.h"
 #include "aigamozuSetting.h"
+#include "agz_common.h"
 
 /////////////////////////////////////////
 //
@@ -32,11 +33,9 @@
 //Serial Connect Setting: XBEE <--> mbed
 XBee xbee(p13,p14);
 ZBRxResponse zbRx = ZBRxResponse();
-XBeeAddress64 remoteAddress = XBeeAddress64(PAN1B1_32H,PAN1B1_32L);
+XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
 
-//Timer
-Timer t;
-int flag=0;
+AGZ_ROBOT robot[4];
 
 /////////////////////////////////////////
 //
@@ -51,7 +50,6 @@
 //
 /////////////////////////////////////////
 int main() {
-    int count = 0;
     //start up time
     wait(3);
     //set pc frequency to 57600bps 
@@ -66,6 +64,16 @@
     const int refresh_Time = 2000; //refresh time in ms
     myGPS.begin(GPS_BAUD_RATE); 
     
+    Timer collect_Timer;
+    const int collect_Time = 200; //when we collect 4 GPS point, we use 
+    int collect_flag = 0;
+    char collect_state = 'a';
+    XBeeAddress64 collect_Address[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
+                                        XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)};
+    XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
+    int id;
+  
+    
     //GPS Send Command
     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
@@ -77,16 +85,19 @@
     AigamozuControlPackets agz(agz_motorShield);
     refresh_Timer.start();
     
+    printf("test\n");
+        
     
     while (true) {
-       
+        
         //Check Xbee Buffer Available
         xbee.readPacket();
             if (xbee.getResponse().isAvailable()) {
                 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
                 xbee.getResponse().getZBRxResponse(zbRx);
-                uint8_t *buf = zbRx.getFrameData(); 
-  
+                uint8_t *buf = zbRx.getFrameData();
+                uint8_t *buf1 = &buf[11]; 
+                 
                  
                  //Check Command Type 
                  switch(agz.checkCommnadType(buf)){
@@ -109,22 +120,40 @@
                     //CommandType -> Send Status
                     case STATUS_REQUEST:{
                             //Create GPS Infomation Packet
-                            agz.createReceiveStatusCommand(1,6,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+                            agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                             //Select Destination
-                            ZBTxRequest tx64request(remoteAddress,agz.packetData,agz.getPacketLength());
+                                ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
                             //Send -> Base
                             xbee.send(tx64request);
                             break;         
           
                         }
+                        
+                    case RECEIVE_STATUS:{
+                            
+                        printf("Receive Status\n");      
+                           
+                        id = buf1[6] - 'A';
+                        robot[id].set_state(buf1[9]);
+                        robot[id].set_LatitudeH(&buf1[13]);
+                        robot[id].set_LatitudeL(&buf1[17]);
+                        robot[id].set_LongitudeH(&buf1[21]);
+                        robot[id].set_LongitudeL(&buf1[25]); 
+                            
+                        printf("%d\n", id);                    
+                        printf("%ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
+                        break;
+                    }
                     
-                        default:
-                        {
+                    default:{
                         break;
-                            }
                     }
                 }
+                    
+            
+                }
             }
+
             
         myGPS.read();
         //recive gps module
@@ -134,18 +163,37 @@
                 continue;   
             }       
         }
-        
         if (refresh_Timer.read_ms() >= refresh_Time) {
             refresh_Timer.reset();
             if (myGPS.fix) {
                 agz.nowStatus = GPS_AVAIL;
                 agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+            
+                printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
             }
             else agz.nowStatus = GPS_UNAVAIL;
+            
         } 
         
-
-        
+        //get base GPS
+        if( collect_Timer.read_ms() >= collect_Time){
+            collect_Timer.reset();
             
-    }
+            printf("Send Request\n");
+                      
+            agz.createRequestCommand('A', collect_state);
+            //Select Destination
+            ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
+            //Send -> Base
+            xbee.send(tx64request);
+            
+            collect_flag++;
+            collect_state++;
+
+            if(collect_flag == 4){
+                collect_state = 'a';
+                collect_flag = 0;
+            }    
+         } 
+    } 
 }
\ No newline at end of file