Diff: agz_base.cpp

Revision:

0:0aada4db04db

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/agz_base.cpp	Wed Jun 18 13:48:27 2014 +0000
@@ -0,0 +1,131 @@
+#include "mbed.h"
+#include "XBee.h"
+
+#include "agz_common.h"
+#include "AigamozuControlPackets.h"
+
+
+#define MY_BASE_NUMBER 0
+
+XBee xbee(p13,p14);
+Serial pc(USBTX, USBRX); // tx, rx
+
+Ticker timer;
+
+//robot data
+AGZ_ROBOT robot[AGZ_NUM_ROBOTS];
+
+
+//function
+//transform packetdata to robot data
+void agz_transform_papcket(uint8_t* packet_data,int id){
+    //set latitude, longitude, state
+    robot[id].set_state(packet_data[9]);
+    robot[id].set_LatitudeH(&packet_data[13]);
+    robot[id].set_LatitudeL(&packet_data[17]);
+    robot[id].set_LongitudeH(&packet_data[21]);
+    robot[id].set_LongitudeL(&packet_data[25]);    
+    
+}
+
+/*---------registere robot--------------*/
+/*
+void agz_register_robot(uint8_*t packet_data){
+    uint8_t robot_id = packet_data[5];
+    robot[robot_id].set_state(0);
+    send_register_ack();
+}
+*/
+
+/*---------timer set----*/
+void agz_send_request(){
+    static uint8_t next_send_address = 0,i;
+    AigamozuControlPackets agz_packet;
+    
+    /*------search next moving robot--------*/
+    for(i = next_send_address;i < AGZ_NUM_ROBOTS;i++){
+        if(robot[i].get_state() != 0xFF){
+            next_send_address = i;
+            break;
+        }
+    }
+    if(i == AGZ_NUM_ROBOTS){
+        for(i = 0;i < AGZ_NUM_ROBOTS;i++){
+            if(robot[i].get_state() != 0xFF){
+                next_send_address = i;
+                break;
+            }
+        }
+    }
+    
+    agz_packet.createRequestCommand(MY_BASE_NUMBER,next_send_address);
+    
+    ZBTxRequest agz_request_packet = ZBTxRequest(Agz_XBee_Remote_Address[next_send_address], agz_packet.packetData, REQUEST_COMMNAD_LENGTH );   
+    
+    xbee.send(agz_request_packet);
+    
+    next_send_address++;
+}
+
+
+/*---------init---------*/
+void agz_base_init(){
+    xbee.begin(57600);
+    pc.baud(57600);
+    timer.attach(&agz_send_request,2.0);
+    
+
+}
+
+int main(void){
+    int sender,reciever;
+    
+    ZBRxResponse xbeeresponse;
+    
+    agz_base_init();
+    
+    wait(1);
+    
+    printf("start\n");
+    
+    
+    robot[0].set_state(0);
+    robot[1].set_state(0);
+    robot[2].set_state(0);
+    robot[3].set_state(0);
+        
+        
+        
+    while(1){
+        xbee.readPacket();
+        
+        //get packet data
+        if(xbee.getResponse().isAvailable() ){ 
+            xbee.getResponse(xbeeresponse);
+            
+            //get data packet
+            if(xbee.getResponse().getApiId() == ZB_RX_RESPONSE){
+                uint8_t *packet_data = xbeeresponse.getData();
+                uint8_t packet_kinds = packet_data[3];
+                
+                //get sensor data
+                if(packet_kinds == 'R'){
+                    sender = packet_data[5] - 'A';
+                    reciever = packet_data[7] - 'a';
+                    agz_transform_papcket(packet_data,sender);
+                    printf("%d,%d,%d,%ld.%ld,%ld.%ld\n",robot[sender].get_state(),sender,reciever,
+                    robot[sender].get_LatitudeH(),robot[sender].get_LatitudeL(),
+                    robot[sender].get_LongitudeH(),robot[sender].get_LongitudeL());
+                }
+                //get register packet
+                if(packet_kinds == 'E'){
+                    //agz_register_robot(packet_data);
+                }
+            }else{
+                //get not rx response
+            }
+        }else{
+          
+        }
+    }
+}
\ No newline at end of file