File content as of revision 12:a7ec1238373e:

#include "XBee_Robot.h"
#include <vector>
#include <algorithm>
#include <list>

//NETWORK CLASS METHODS
NetworkNode::NetworkNode(std::vector<uint8_t> & addrIn, int indexIn){
    addr = addrIn;
    nodeNum = indexIn;
}   

NetworkNode::NetworkNode(std::vector<uint8_t> & addrIn, std::vector<uint8_t> & addr64In, int indexIn){
    addr = addrIn;
    nodeNum = indexIn;
    addr64 = addr64In;
}

std::vector<uint8_t> NetworkNode::getAddr(){ //returns 16 bit address of node
    return addr;
}

std::vector<uint8_t> NetworkNode::getAddr64(){ //returns 64 bit address of node
    return addr64;
}

int NetworkNode::getIndex(){ //returns index of node
    return nodeNum;
}

int NetworkNode::getX(){
    return x;
}

int NetworkNode::getY(){
    return y;
}

int NetworkNode::getHeading(){
    return heading;
}

void NetworkNode::setCoordinates(int x_in, int y_in, int heading_in){
 x = x_in;
 y = y_in;
 heading = heading_in;
}
 
void NetworkNode::setIndex(int indexIn){ //sets index of node
    nodeNum = indexIn;
} 

void NetworkNode::setAddr(std::vector<uint8_t> & addrIn){ //sets address of node
    addr = addrIn;
}  

/**************************************************************************************************/

//XBEE ROBOT CLASS METHODS
XBee_Robot::XBee_Robot(PinName _txIn, PinName _rxIn): dataLink(_txIn,_rxIn){
    dataLink.attach(this,&XBee_Robot::Rx_interrupt, Serial::RxIrq); //set interrupt function on receive pin
    ATQuery(0x4D,0x59); //create AT query with AT command 'MY' to query own 16 bit network address
    commandFlag = 0; //set command flag to 0 on initialisation
    finishedFlag = 0; //set finished flag to 0 on intialisation
}

void XBee_Robot::Rx_interrupt()
{
    std::vector<uint8_t> Rx_buffer;
    while(dataLink.readable()){
        Rx_buffer.push_back(dataLink.getc());//add each incoming byte to buffer
        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)
    }
    
    //Check valid packet delimeter and checksum
    if((Rx_buffer[0] == 0x7E) && (Rx_buffer[Rx_buffer.size()] == calculateChecksum(Rx_buffer)))
        RxPacketControl(Rx_buffer); //call packet control function
    else
        printf("Packet failed delimeter and checksum check");
}

void XBee_Robot::transmitRequest(uint8_t *BitAddress64, uint8_t *BitAddress16, uint8_t broadcastRadius, uint8_t options, uint8_t *data,size_t dataLength)
{
    //calculate checksum
    uint16_t length = 0x0E + dataLength; //calculate length of packet (14 + data length)
    uint8_t lengthu = length >>8; //upper 8 bits
    uint8_t lengthl = length & 0xFF; //lower 8 bits
    
    
    std::vector<uint8_t> transmitRequestPacket; //create new vector packet
    //populate packet 
    transmitRequestPacket.push_back(0x7E); //start delimeter
    transmitRequestPacket.push_back(lengthu); //upper byte of length
    transmitRequestPacket.push_back(lengthl); //lower byte of length
    transmitRequestPacket.push_back(0x10); //API ID (transmit request)
    transmitRequestPacket.push_back(0x01); //channel ID
    transmitRequestPacket.insert(transmitRequestPacket.end(), BitAddress64, BitAddress64+8); //64 bit destination address
    transmitRequestPacket.insert(transmitRequestPacket.end(), BitAddress16, BitAddress16+2); //16 bit network address
    transmitRequestPacket.push_back(broadcastRadius); //broadcast radius (0 = max hops)
    transmitRequestPacket.push_back(options); //additional options for packet
    transmitRequestPacket.insert(transmitRequestPacket.end(), data, data+dataLength); //data
    uint8_t checksum = calculateChecksum(transmitRequestPacket);
    transmitRequestPacket.push_back(checksum); //calculate and add checksum  
          
    for (int i = 0; i < transmitRequestPacket.size(); i++){
        dataLink.printf("%c",transmitRequestPacket[i]); //send packet
    }
}

void XBee_Robot::ATQuery(uint8_t ATu, uint8_t ATl)
{
    //calculate checksum
    uint8_t lengthu = 0; //upper 8 bits of length
    uint8_t lengthl = 0x04; //lower 8 bits of length
    
    
    std::vector<uint8_t> ATRequestPacket; //create new vector packet
    //populate packet 
    ATRequestPacket.push_back(0x7E); //start delimeter
    ATRequestPacket.push_back(lengthu); //upper byte of length
    ATRequestPacket.push_back(lengthl); //lower byte of length
    ATRequestPacket.push_back(0x08); //API ID (AT request)
    ATRequestPacket.push_back(0x52); //channel ID
    ATRequestPacket.push_back(ATu); //AT command (upper byte)
    ATRequestPacket.push_back(ATl); //AT command (lower byte)
    uint8_t checksum = calculateChecksum(ATRequestPacket);
    ATRequestPacket.push_back(checksum); //calculate and add checksum  
          
    for (int i = 0; i < ATRequestPacket.size(); i++){
        dataLink.printf("%c",ATRequestPacket[i]); //send packet
    }
}

uint8_t XBee_Robot::calculateChecksum(std::vector<uint8_t> & packet)
{
    uint8_t checksum = 0xFF; //start with FF as last byte of sum is subtracted from FF
    for (int i = 3; i < packet.size(); i++)
        checksum -= packet[i];
    return checksum;
    
}

void XBee_Robot::RxPacketControl(std::vector<uint8_t> & packet)
{        
    uint8_t command = packet[3]; //take API address
    switch (command) { //index for different commands
        case 0x90:{ //Receive packet command
        
            std::vector<uint8_t> source_addr16; //create new vector to 16 bit store source address
            std::vector<uint8_t> source_addr64; //create new vector to 64 bit store source address
            source_addr16.insert(source_addr16.end(), packet.begin() + 12, packet.begin() + 14); //insert source address part of packet into new vector
            source_addr64.insert(source_addr64.end(), packet.begin() + 4, packet.begin() + 12); //insert source address part of packet into new vector
            checkSourceAddr(source_addr16,source_addr64);
            
            std::vector<uint8_t> data; //create new vector to store data
            data.insert(data.end(), packet.begin() + 15, packet.end() -1); //insert data part of packet into new vector
            /*for(int i = 0; i<data.size();i++){
                printf("Data: %d\n",(int)data[i]); //display data from packet
            }*/
            RxDataHandler(data);
            
            break;
        }
        case 0x88:{ //AT response packet command
            if(packet[7] == 0x00){ //if packet command status is ok                    
                std::vector<uint8_t> data; //create new vector to store data
                data.insert(data.end(), packet.begin() + 8, packet.end() -1); //insert data part of packet into new vector
                if((packet[5] == 0x4D) & (packet[6] == 0x59)){ //if AT command is 'MY'
                    checkSourceAddr(data); //call function to enter own network address in node_list
                }
            }
            break;
        }
        case 0x8B: { //Tx status packet command
            if(packet[8] == 0 )//if delivery status is 00 (success)
                printf("Packet successfully transmitted to 16 bit destination address: %c%c",packet[5],packet[6]);
            else
                printf("Packet delivery failed (status %c)",packet[8]);
            break;
        }
        default:
            printf("Received API address not recognised: %c",command);
    }
}

void XBee_Robot::checkSourceAddr(std::vector<uint8_t> & addr, std::vector<uint8_t> & addr64)
{
    bool exists = false;
    for (int i = 0; i<node_list.size();i++){ //search each entry in node_list for matching address
        currentIndex = i; //update currentIndex
        if(node_list[i].getAddr() == addr){
            exists = true;
            printf("Recognised node %d\n",node_list[i].getIndex()); //print node number
        }
    }
    if (exists == false){ //add new address to list if no match found
        currentIndex++; //increment current index for new entry
        NetworkNode newNode(addr,addr64,node_list.size());//create new node
        node_list.push_back(newNode); //add new node to list
        printf("New address added: Node %d\n",(int)node_list.size()-1); 
    }
        
}

void XBee_Robot::checkSourceAddr(std::vector<uint8_t> & addr)
{
    bool exists = false;
    for (int i = 0; i<node_list.size();i++){ //search each entry in node_list for matching address
        currentIndex = i; //update currentIndex
        if(node_list[i].getAddr() == addr){
            exists = true;
            printf("Recognised node %d\n",node_list[i].getIndex()); //print node number
        }
    }
    if (exists == false){ //add new address to list if no match found
        currentIndex++; //increment current index for new entry
        NetworkNode newNode(addr,node_list.size()); //create new node
        node_list.push_back(newNode); //add new node to list
        printf("New address added: Node %d\n",(int)node_list.size()-1); 
    }
        
}

void XBee_Robot::RxDataHandler(std::vector<uint8_t> & packet)
{        
    uint8_t command = packet[0]; //take data command
    switch (command) { //index for different commands
        case 0xFF:{ //Receive proximity command
            uint16_t prox = ((uint16_t)packet[1] << 8) | packet[2]; //create word to assemble upper and lower proximity data bytes
            printf("Proximity: %d\n",(int)prox); //display data from packet
            break;
        }
        case 0x01:{ //Receive location command
            node_list[currentIndex].setCoordinates(convSigned((int)packet[1]),convSigned((int)packet[2]),convSigned((int)packet[3])); //update coordinates for corresponding node
            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
            break;
        }
        case 0x02: { //Receive destination coordinates from coordinator command
            uint16_t comX = ((uint16_t)packet[1] << 8) | packet[2]; //create word to assemble upper and lower byte of x coordinate
            commandX = int(comX); //set commandX to received coordinate
            uint16_t comY = ((uint16_t)packet[3] << 8) | packet[4];
            commandY = int(comY);
            commandFlag = 1; //set command flag to indicate received coordinates
            break;
        }
        case 0xFA: { //Receive router robot finished command
            printf("Finished flag set\n");
            finishedFlag = 1; //set command flag to indicate received coordinates
            break;
        }
        case 0xFC: { //Receive obstacle coordinate command
            printf("Obstacle coordinate received at %d, %d\n",convSigned((int)packet[1]),convSigned((int)packet[2]));
            std::vector <int> coordinate;
            coordinate.push_back(convSigned((int)packet[1]));
            coordinate.push_back(convSigned((int)packet[2]));
            obstacles.push_back(coordinate);
            break;
        }
        default:
            printf("Received data command not recognised: %c",command);
    }
}

int XBee_Robot::getCommandX(){
    return commandX;
}

int XBee_Robot::getCommandY(){
    return commandY;
}

int XBee_Robot::getCommandFlag(){
    return commandFlag; //return value of command flag
}

int XBee_Robot::getFinishedFlag(){
    return finishedFlag; //return value of finished flag
}

void XBee_Robot::resetFinishedFlag(){
    finishedFlag = 0; //reset finished flag
}

void XBee_Robot::resetCommandFlag(){
    commandFlag = 0; //reset command flag
}

int XBee_Robot::checkObstacle(int x_in, int y_in){
  for (int i = 0; i<obstacles.size();i++){ //search each entry in node_list for matching node
        if((obstacles[i][0] == x_in)&&(obstacles[i][1] == y_in)){
            return 1; //return 1 if obstacle exists at given coordinates
            printf("Obstacle found at %d,%d\n",obstacles[i][0],obstacles[i][1]); //print node number
        }
    }
    return 0; //return 0 if no obstacle exists at given coordinates
}

int XBee_Robot::convSigned(int int_in){
    if(int_in >= 128)
        return int_in-256; //if value is greater or equal to 128 convert to negative number
    return int_in;
}