aigamo get data

Dependencies:   AigamozuControlPackets XBee agz_common mbed

Fork of agz_base_ver2 by K M

agz_base.cpp

Committer:
kityann
Date:
2014-06-18
Revision:
1:533008368d76
Parent:
0:0aada4db04db

File content as of revision 1:533008368d76:

#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{
          
        }
    }
}