XBee_Robot.cpp

00001 #include "XBee_Robot.h"
00002 #include <vector>
00003 #include <algorithm>
00004 #include <list>
00005 
00006 //NETWORK CLASS METHODS
00007 NetworkNode::NetworkNode(std::vector<uint8_t> & addrIn, int indexIn){
00008     addr = addrIn;
00009     nodeNum = indexIn;
00010 }   
00011 
00012 NetworkNode::NetworkNode(std::vector<uint8_t> & addrIn, std::vector<uint8_t> & addr64In, int indexIn){
00013     addr = addrIn;
00014     nodeNum = indexIn;
00015     addr64 = addr64In;
00016 }
00017 
00018 std::vector<uint8_t> NetworkNode::getAddr(){ //returns 16 bit address of node
00019     return addr;
00020 }
00021 
00022 std::vector<uint8_t> NetworkNode::getAddr64(){ //returns 64 bit address of node
00023     return addr64;
00024 }
00025 
00026 int NetworkNode::getIndex(){ //returns index of node
00027     return nodeNum;
00028 }
00029 
00030 int NetworkNode::getX(){
00031     return x;
00032 }
00033 
00034 int NetworkNode::getY(){
00035     return y;
00036 }
00037 
00038 int NetworkNode::getHeading(){
00039     return heading;
00040 }
00041 
00042 void NetworkNode::setCoordinates(int x_in, int y_in, int heading_in){
00043  x = x_in;
00044  y = y_in;
00045  heading = heading_in;
00046 }
00047  
00048 void NetworkNode::setIndex(int indexIn){ //sets index of node
00049     nodeNum = indexIn;
00050 } 
00051 
00052 void NetworkNode::setAddr(std::vector<uint8_t> & addrIn){ //sets address of node
00053     addr = addrIn;
00054 }  
00055 
00056 /**************************************************************************************************/
00057 
00058 //XBEE ROBOT CLASS METHODS
00059 XBee_Robot::XBee_Robot(PinName _txIn, PinName _rxIn): dataLink(_txIn,_rxIn){
00060     dataLink.attach(this,&XBee_Robot::Rx_interrupt, Serial::RxIrq); //set interrupt function on receive pin
00061     ATQuery(0x4D,0x59); //create AT query with AT command 'MY' to query own 16 bit network address
00062     commandFlag = 0; //set command flag to 0 on initialisation
00063     finishedFlag = 0; //set finished flag to 0 on intialisation
00064 }
00065 
00066 void XBee_Robot::Rx_interrupt()
00067 {
00068     std::vector<uint8_t> Rx_buffer;
00069     while(dataLink.readable()){
00070         Rx_buffer.push_back(dataLink.getc());//add each incoming byte to buffer
00071         wait(0.00107); //wait for long enough so the next digit is recognised in the same stream (updated from 0.0011 to accomodate for 2 bytes of data)
00072     }
00073     
00074     //Check valid packet delimeter and checksum
00075     if((Rx_buffer[0] == 0x7E) && (Rx_buffer[Rx_buffer.size()] == calculateChecksum(Rx_buffer)))
00076         RxPacketControl(Rx_buffer); //call packet control function
00077     else
00078         printf("Packet failed delimeter and checksum check");
00079 }
00080 
00081 void XBee_Robot::transmitRequest(uint8_t *BitAddress64, uint8_t *BitAddress16, uint8_t broadcastRadius, uint8_t options, uint8_t *data,size_t dataLength)
00082 {
00083     //calculate checksum
00084     uint16_t length = 0x0E + dataLength; //calculate length of packet (14 + data length)
00085     uint8_t lengthu = length >>8; //upper 8 bits
00086     uint8_t lengthl = length & 0xFF; //lower 8 bits
00087     
00088     
00089     std::vector<uint8_t> transmitRequestPacket; //create new vector packet
00090     //populate packet 
00091     transmitRequestPacket.push_back(0x7E); //start delimeter
00092     transmitRequestPacket.push_back(lengthu); //upper byte of length
00093     transmitRequestPacket.push_back(lengthl); //lower byte of length
00094     transmitRequestPacket.push_back(0x10); //API ID (transmit request)
00095     transmitRequestPacket.push_back(0x01); //channel ID
00096     transmitRequestPacket.insert(transmitRequestPacket.end(), BitAddress64, BitAddress64+8); //64 bit destination address
00097     transmitRequestPacket.insert(transmitRequestPacket.end(), BitAddress16, BitAddress16+2); //16 bit network address
00098     transmitRequestPacket.push_back(broadcastRadius); //broadcast radius (0 = max hops)
00099     transmitRequestPacket.push_back(options); //additional options for packet
00100     transmitRequestPacket.insert(transmitRequestPacket.end(), data, data+dataLength); //data
00101     uint8_t checksum = calculateChecksum(transmitRequestPacket);
00102     transmitRequestPacket.push_back(checksum); //calculate and add checksum  
00103           
00104     for (int i = 0; i < transmitRequestPacket.size(); i++){
00105         dataLink.printf("%c",transmitRequestPacket[i]); //send packet
00106     }
00107 }
00108 
00109 void XBee_Robot::ATQuery(uint8_t ATu, uint8_t ATl)
00110 {
00111     //calculate checksum
00112     uint8_t lengthu = 0; //upper 8 bits of length
00113     uint8_t lengthl = 0x04; //lower 8 bits of length
00114     
00115     
00116     std::vector<uint8_t> ATRequestPacket; //create new vector packet
00117     //populate packet 
00118     ATRequestPacket.push_back(0x7E); //start delimeter
00119     ATRequestPacket.push_back(lengthu); //upper byte of length
00120     ATRequestPacket.push_back(lengthl); //lower byte of length
00121     ATRequestPacket.push_back(0x08); //API ID (AT request)
00122     ATRequestPacket.push_back(0x52); //channel ID
00123     ATRequestPacket.push_back(ATu); //AT command (upper byte)
00124     ATRequestPacket.push_back(ATl); //AT command (lower byte)
00125     uint8_t checksum = calculateChecksum(ATRequestPacket);
00126     ATRequestPacket.push_back(checksum); //calculate and add checksum  
00127           
00128     for (int i = 0; i < ATRequestPacket.size(); i++){
00129         dataLink.printf("%c",ATRequestPacket[i]); //send packet
00130     }
00131 }
00132 
00133 uint8_t XBee_Robot::calculateChecksum(std::vector<uint8_t> & packet)
00134 {
00135     uint8_t checksum = 0xFF; //start with FF as last byte of sum is subtracted from FF
00136     for (int i = 3; i < packet.size(); i++)
00137         checksum -= packet[i];
00138     return checksum;
00139     
00140 }
00141 
00142 void XBee_Robot::RxPacketControl(std::vector<uint8_t> & packet)
00143 {        
00144     uint8_t command = packet[3]; //take API address
00145     switch (command) { //index for different commands
00146         case 0x90:{ //Receive packet command
00147         
00148             std::vector<uint8_t> source_addr16; //create new vector to 16 bit store source address
00149             std::vector<uint8_t> source_addr64; //create new vector to 64 bit store source address
00150             source_addr16.insert(source_addr16.end(), packet.begin() + 12, packet.begin() + 14); //insert source address part of packet into new vector
00151             source_addr64.insert(source_addr64.end(), packet.begin() + 4, packet.begin() + 12); //insert source address part of packet into new vector
00152             checkSourceAddr(source_addr16,source_addr64);
00153             
00154             std::vector<uint8_t> data; //create new vector to store data
00155             data.insert(data.end(), packet.begin() + 15, packet.end() -1); //insert data part of packet into new vector
00156             /*for(int i = 0; i<data.size();i++){
00157                 printf("Data: %d\n",(int)data[i]); //display data from packet
00158             }*/
00159             RxDataHandler(data);
00160             
00161             break;
00162         }
00163         case 0x88:{ //AT response packet command
00164             if(packet[7] == 0x00){ //if packet command status is ok                    
00165                 std::vector<uint8_t> data; //create new vector to store data
00166                 data.insert(data.end(), packet.begin() + 8, packet.end() -1); //insert data part of packet into new vector
00167                 if((packet[5] == 0x4D) & (packet[6] == 0x59)){ //if AT command is 'MY'
00168                     checkSourceAddr(data); //call function to enter own network address in node_list
00169                 }
00170             }
00171             break;
00172         }
00173         case 0x8B: { //Tx status packet command
00174             if(packet[8] == 0 )//if delivery status is 00 (success)
00175                 printf("Packet successfully transmitted to 16 bit destination address: %c%c",packet[5],packet[6]);
00176             else
00177                 printf("Packet delivery failed (status %c)",packet[8]);
00178             break;
00179         }
00180         default:
00181             printf("Received API address not recognised: %c",command);
00182     }
00183 }
00184 
00185 void XBee_Robot::checkSourceAddr(std::vector<uint8_t> & addr, std::vector<uint8_t> & addr64)
00186 {
00187     bool exists = false;
00188     for (int i = 0; i<node_list.size();i++){ //search each entry in node_list for matching address
00189         currentIndex = i; //update currentIndex
00190         if(node_list[i].getAddr() == addr){
00191             exists = true;
00192             printf("Recognised node %d\n",node_list[i].getIndex()); //print node number
00193         }
00194     }
00195     if (exists == false){ //add new address to list if no match found
00196         currentIndex++; //increment current index for new entry
00197         NetworkNode newNode(addr,addr64,node_list.size());//create new node
00198         node_list.push_back(newNode); //add new node to list
00199         printf("New address added: Node %d\n",(int)node_list.size()-1); 
00200     }
00201         
00202 }
00203 
00204 void XBee_Robot::checkSourceAddr(std::vector<uint8_t> & addr)
00205 {
00206     bool exists = false;
00207     for (int i = 0; i<node_list.size();i++){ //search each entry in node_list for matching address
00208         currentIndex = i; //update currentIndex
00209         if(node_list[i].getAddr() == addr){
00210             exists = true;
00211             printf("Recognised node %d\n",node_list[i].getIndex()); //print node number
00212         }
00213     }
00214     if (exists == false){ //add new address to list if no match found
00215         currentIndex++; //increment current index for new entry
00216         NetworkNode newNode(addr,node_list.size()); //create new node
00217         node_list.push_back(newNode); //add new node to list
00218         printf("New address added: Node %d\n",(int)node_list.size()-1); 
00219     }
00220         
00221 }
00222 
00223 void XBee_Robot::RxDataHandler(std::vector<uint8_t> & packet)
00224 {        
00225     uint8_t command = packet[0]; //take data command
00226     switch (command) { //index for different commands
00227         case 0xFF:{ //Receive proximity command
00228             uint16_t prox = ((uint16_t)packet[1] << 8) | packet[2]; //create word to assemble upper and lower proximity data bytes
00229             printf("Proximity: %d\n",(int)prox); //display data from packet
00230             break;
00231         }
00232         case 0x01:{ //Receive location command
00233             node_list[currentIndex].setCoordinates(convSigned((int)packet[1]),convSigned((int)packet[2]),convSigned((int)packet[3])); //update coordinates for corresponding node
00234             printf("Receive Location Command, X = %d, Y = %d, Heading = %d\n",node_list[currentIndex].getX(),node_list[currentIndex].getY(),node_list[currentIndex].getHeading()); //display data from packet
00235             break;
00236         }
00237         case 0x02: { //Receive destination coordinates from coordinator command
00238             uint16_t comX = ((uint16_t)packet[1] << 8) | packet[2]; //create word to assemble upper and lower byte of x coordinate
00239             commandX = int(comX); //set commandX to received coordinate
00240             uint16_t comY = ((uint16_t)packet[3] << 8) | packet[4];
00241             commandY = int(comY);
00242             commandFlag = 1; //set command flag to indicate received coordinates
00243             break;
00244         }
00245         case 0xFA: { //Receive router robot finished command
00246             printf("Finished flag set\n");
00247             finishedFlag = 1; //set command flag to indicate received coordinates
00248             break;
00249         }
00250         case 0xFC: { //Receive obstacle coordinate command
00251             printf("Obstacle coordinate received at %d, %d\n",convSigned((int)packet[1]),convSigned((int)packet[2]));
00252             std::vector <int> coordinate;
00253             coordinate.push_back(convSigned((int)packet[1]));
00254             coordinate.push_back(convSigned((int)packet[2]));
00255             obstacles.push_back(coordinate);
00256             break;
00257         }
00258         default:
00259             printf("Received data command not recognised: %c",command);
00260     }
00261 }
00262 
00263 int XBee_Robot::getCommandX(){
00264     return commandX;
00265 }
00266 
00267 int XBee_Robot::getCommandY(){
00268     return commandY;
00269 }
00270 
00271 int XBee_Robot::getCommandFlag(){
00272     return commandFlag; //return value of command flag
00273 }
00274 
00275 int XBee_Robot::getFinishedFlag(){
00276     return finishedFlag; //return value of finished flag
00277 }
00278 
00279 void XBee_Robot::resetFinishedFlag(){
00280     finishedFlag = 0; //reset finished flag
00281 }
00282 
00283 void XBee_Robot::resetCommandFlag(){
00284     commandFlag = 0; //reset command flag
00285 }
00286 
00287 int XBee_Robot::checkObstacle(int x_in, int y_in){
00288   for (int i = 0; i<obstacles.size();i++){ //search each entry in node_list for matching node
00289         if((obstacles[i][0] == x_in)&&(obstacles[i][1] == y_in)){
00290             return 1; //return 1 if obstacle exists at given coordinates
00291             printf("Obstacle found at %d,%d\n",obstacles[i][0],obstacles[i][1]); //print node number
00292         }
00293     }
00294     return 0; //return 0 if no obstacle exists at given coordinates
00295 }
00296 
00297 int XBee_Robot::convSigned(int int_in){
00298     if(int_in >= 128)
00299         return int_in-256; //if value is greater or equal to 128 convert to negative number
00300     return int_in;
00301 }