![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
20150512 Nakazawa
Dependencies: AigamozuControlPackets XBee agz_common mbed
Fork of agz_base_ver2 by
Diff: agz_base.cpp
- Revision:
- 0:0aada4db04db
diff -r 000000000000 -r 0aada4db04db agz_base.cpp --- /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