File content as of revision 8:7e936dc02dec:

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

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

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

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

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

void NetworkNode::setCoordinates(int x_in, int y_in){
 x = x_in;
 y = y_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){
    ATQuery(0x4D,0x59); //create AT query with AT command 'MY' to query own 16 bit network address
}

void XBee_Robot::setRxInterrupt()
{
    dataLink.attach(this,&XBee_Robot::Rx_interrupt, Serial::RxIrq);
}

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
}

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
        printf("%c",transmitRequestPacket[i]);
    }
}

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 store source address
            source_addr16.insert(source_addr16.end(), packet.begin() + 13, packet.begin() + 15); //insert source address part of packet into new vector
            checkSourceAddr(source_addr16);
            
            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;
        }
        default:
            printf("Received API address not recognised: %c",command);
    }
}

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((int)packet[1],(int)packet[2]); //update coordinates for corresponding node
            printf("X = %d, Y = %d\n",node_list[currentIndex].getX(),node_list[currentIndex].getY()); //display data from packet
            break;
        }
        default:
            printf("Received data command not recognised: %c",command);
    }
}