Sam Leighton / XBee_Robot

Library for XBee API targeted toward functions with specific use in conjunction with the Pololu 3pi Robot. Work in Progress

XBee_Robot.cpp@8:7e936dc02dec, 2016-01-30 (annotated)

Committer:
sleighton
Date:
Sat Jan 30 21:20:00 2016 +0000
Revision:
8:7e936dc02dec
Parent:
7:c3acafdb70c0
Child:
9:d5e7e772d5a4
Handles coordinates, updates node list accordingly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sleighton 2:5040ec01dba1 1 #include "XBee_Robot.h"
sleighton 2:5040ec01dba1 2 #include <vector>
sleighton 3:cf539cfd3d59 3 #include <algorithm>
sleighton 3:cf539cfd3d59 4 #include <list>
sleighton 2:5040ec01dba1 5
sleighton 4:af08c7749f9d 6 //NETWORK CLASS METHODS
sleighton 4:af08c7749f9d 7 NetworkNode::NetworkNode(std::vector<uint8_t> & addrIn, int indexIn){
sleighton 4:af08c7749f9d 8 addr = addrIn;
sleighton 4:af08c7749f9d 9 nodeNum = indexIn;
sleighton 4:af08c7749f9d 10 }
sleighton 4:af08c7749f9d 11
sleighton 4:af08c7749f9d 12 std::vector<uint8_t> NetworkNode::getAddr(){ //returns address of node
sleighton 4:af08c7749f9d 13 return addr;
sleighton 2:5040ec01dba1 14 }
sleighton 2:5040ec01dba1 15
sleighton 4:af08c7749f9d 16 int NetworkNode::getIndex(){ //returns index of node
sleighton 4:af08c7749f9d 17 return nodeNum;
sleighton 4:af08c7749f9d 18 }
sleighton 8:7e936dc02dec 19
sleighton 8:7e936dc02dec 20 int NetworkNode::getX(){
sleighton 8:7e936dc02dec 21 return x;
sleighton 8:7e936dc02dec 22 }
sleighton 8:7e936dc02dec 23
sleighton 8:7e936dc02dec 24 int NetworkNode::getY(){
sleighton 8:7e936dc02dec 25 return y;
sleighton 8:7e936dc02dec 26 }
sleighton 8:7e936dc02dec 27
sleighton 8:7e936dc02dec 28 void NetworkNode::setCoordinates(int x_in, int y_in){
sleighton 8:7e936dc02dec 29 x = x_in;
sleighton 8:7e936dc02dec 30 y = y_in;
sleighton 8:7e936dc02dec 31 }
sleighton 4:af08c7749f9d 32
sleighton 4:af08c7749f9d 33 void NetworkNode::setIndex(int indexIn){ //sets index of node
sleighton 4:af08c7749f9d 34 nodeNum = indexIn;
sleighton 4:af08c7749f9d 35 }
sleighton 4:af08c7749f9d 36
sleighton 4:af08c7749f9d 37 void NetworkNode::setAddr(std::vector<uint8_t> & addrIn){ //sets address of node
sleighton 4:af08c7749f9d 38 addr = addrIn;
sleighton 4:af08c7749f9d 39 }
sleighton 4:af08c7749f9d 40
sleighton 6:fb0316cafaa6 41 /**************************************************************************************************/
sleighton 6:fb0316cafaa6 42
sleighton 4:af08c7749f9d 43 //XBEE ROBOT CLASS METHODS
sleighton 6:fb0316cafaa6 44 XBee_Robot::XBee_Robot(PinName _txIn, PinName _rxIn): dataLink(_txIn,_rxIn){
sleighton 7:c3acafdb70c0 45 ATQuery(0x4D,0x59); //create AT query with AT command 'MY' to query own 16 bit network address
sleighton 6:fb0316cafaa6 46 }
sleighton 4:af08c7749f9d 47
sleighton 2:5040ec01dba1 48 void XBee_Robot::setRxInterrupt()
sleighton 2:5040ec01dba1 49 {
sleighton 2:5040ec01dba1 50 dataLink.attach(this,&XBee_Robot::Rx_interrupt, Serial::RxIrq);
sleighton 2:5040ec01dba1 51 }
sleighton 2:5040ec01dba1 52
sleighton 2:5040ec01dba1 53 void XBee_Robot::Rx_interrupt()
sleighton 2:5040ec01dba1 54 {
sleighton 3:cf539cfd3d59 55 std::vector<uint8_t> Rx_buffer;
sleighton 2:5040ec01dba1 56 while(dataLink.readable()){
sleighton 3:cf539cfd3d59 57 Rx_buffer.push_back(dataLink.getc());//add each incoming byte to buffer
sleighton 6:fb0316cafaa6 58 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)
sleighton 2:5040ec01dba1 59 }
sleighton 3:cf539cfd3d59 60
sleighton 3:cf539cfd3d59 61 //Check valid packet delimeter and checksum
sleighton 3:cf539cfd3d59 62 if((Rx_buffer[0] == 0x7E) && (Rx_buffer[Rx_buffer.size()] == calculateChecksum(Rx_buffer)))
sleighton 3:cf539cfd3d59 63 RxPacketControl(Rx_buffer); //call packet control function
sleighton 2:5040ec01dba1 64 }
sleighton 2:5040ec01dba1 65
sleighton 2:5040ec01dba1 66 void XBee_Robot::transmitRequest(uint8_t *BitAddress64, uint8_t *BitAddress16, uint8_t broadcastRadius, uint8_t options, uint8_t *data,size_t dataLength)
sleighton 2:5040ec01dba1 67 {
sleighton 2:5040ec01dba1 68 //calculate checksum
sleighton 2:5040ec01dba1 69 uint16_t length = 0x0E + dataLength; //calculate length of packet (14 + data length)
sleighton 2:5040ec01dba1 70 uint8_t lengthu = length >>8; //upper 8 bits
sleighton 2:5040ec01dba1 71 uint8_t lengthl = length & 0xFF; //lower 8 bits
sleighton 2:5040ec01dba1 72
sleighton 2:5040ec01dba1 73
sleighton 2:5040ec01dba1 74 std::vector<uint8_t> transmitRequestPacket; //create new vector packet
sleighton 2:5040ec01dba1 75 //populate packet
sleighton 2:5040ec01dba1 76 transmitRequestPacket.push_back(0x7E); //start delimeter
sleighton 2:5040ec01dba1 77 transmitRequestPacket.push_back(lengthu); //upper byte of length
sleighton 2:5040ec01dba1 78 transmitRequestPacket.push_back(lengthl); //lower byte of length
sleighton 2:5040ec01dba1 79 transmitRequestPacket.push_back(0x10); //API ID (transmit request)
sleighton 2:5040ec01dba1 80 transmitRequestPacket.push_back(0x01); //channel ID
sleighton 2:5040ec01dba1 81 transmitRequestPacket.insert(transmitRequestPacket.end(), BitAddress64, BitAddress64+8); //64 bit destination address
sleighton 2:5040ec01dba1 82 transmitRequestPacket.insert(transmitRequestPacket.end(), BitAddress16, BitAddress16+2); //16 bit network address
sleighton 2:5040ec01dba1 83 transmitRequestPacket.push_back(broadcastRadius); //broadcast radius (0 = max hops)
sleighton 2:5040ec01dba1 84 transmitRequestPacket.push_back(options); //additional options for packet
sleighton 2:5040ec01dba1 85 transmitRequestPacket.insert(transmitRequestPacket.end(), data, data+dataLength); //data
sleighton 2:5040ec01dba1 86 uint8_t checksum = calculateChecksum(transmitRequestPacket);
sleighton 2:5040ec01dba1 87 transmitRequestPacket.push_back(checksum); //calculate and add checksum
sleighton 2:5040ec01dba1 88
sleighton 2:5040ec01dba1 89 for (int i = 0; i < transmitRequestPacket.size(); i++){
sleighton 2:5040ec01dba1 90 dataLink.printf("%c",transmitRequestPacket[i]); //send packet
sleighton 2:5040ec01dba1 91 printf("%c",transmitRequestPacket[i]);
sleighton 2:5040ec01dba1 92 }
sleighton 2:5040ec01dba1 93 }
sleighton 2:5040ec01dba1 94
sleighton 6:fb0316cafaa6 95 void XBee_Robot::ATQuery(uint8_t ATu, uint8_t ATl)
sleighton 6:fb0316cafaa6 96 {
sleighton 6:fb0316cafaa6 97 //calculate checksum
sleighton 6:fb0316cafaa6 98 uint8_t lengthu = 0; //upper 8 bits of length
sleighton 6:fb0316cafaa6 99 uint8_t lengthl = 0x04; //lower 8 bits of length
sleighton 6:fb0316cafaa6 100
sleighton 6:fb0316cafaa6 101
sleighton 6:fb0316cafaa6 102 std::vector<uint8_t> ATRequestPacket; //create new vector packet
sleighton 6:fb0316cafaa6 103 //populate packet
sleighton 6:fb0316cafaa6 104 ATRequestPacket.push_back(0x7E); //start delimeter
sleighton 6:fb0316cafaa6 105 ATRequestPacket.push_back(lengthu); //upper byte of length
sleighton 6:fb0316cafaa6 106 ATRequestPacket.push_back(lengthl); //lower byte of length
sleighton 6:fb0316cafaa6 107 ATRequestPacket.push_back(0x08); //API ID (AT request)
sleighton 6:fb0316cafaa6 108 ATRequestPacket.push_back(0x52); //channel ID
sleighton 6:fb0316cafaa6 109 ATRequestPacket.push_back(ATu); //AT command (upper byte)
sleighton 6:fb0316cafaa6 110 ATRequestPacket.push_back(ATl); //AT command (lower byte)
sleighton 6:fb0316cafaa6 111 uint8_t checksum = calculateChecksum(ATRequestPacket);
sleighton 6:fb0316cafaa6 112 ATRequestPacket.push_back(checksum); //calculate and add checksum
sleighton 6:fb0316cafaa6 113
sleighton 6:fb0316cafaa6 114 for (int i = 0; i < ATRequestPacket.size(); i++){
sleighton 6:fb0316cafaa6 115 dataLink.printf("%c",ATRequestPacket[i]); //send packet
sleighton 6:fb0316cafaa6 116 }
sleighton 6:fb0316cafaa6 117 }
sleighton 6:fb0316cafaa6 118
sleighton 2:5040ec01dba1 119 uint8_t XBee_Robot::calculateChecksum(std::vector<uint8_t> & packet)
sleighton 2:5040ec01dba1 120 {
sleighton 2:5040ec01dba1 121 uint8_t checksum = 0xFF; //start with FF as last byte of sum is subtracted from FF
sleighton 2:5040ec01dba1 122 for (int i = 3; i < packet.size(); i++)
sleighton 2:5040ec01dba1 123 checksum -= packet[i];
sleighton 2:5040ec01dba1 124 return checksum;
sleighton 2:5040ec01dba1 125
sleighton 3:cf539cfd3d59 126 }
sleighton 3:cf539cfd3d59 127
sleighton 3:cf539cfd3d59 128 void XBee_Robot::RxPacketControl(std::vector<uint8_t> & packet)
sleighton 6:fb0316cafaa6 129 {
sleighton 3:cf539cfd3d59 130 uint8_t command = packet[3]; //take API address
sleighton 3:cf539cfd3d59 131 switch (command) { //index for different commands
sleighton 3:cf539cfd3d59 132 case 0x90:{ //Receive packet command
sleighton 3:cf539cfd3d59 133
sleighton 6:fb0316cafaa6 134 std::vector<uint8_t> source_addr16; //create new vector to store source address
sleighton 6:fb0316cafaa6 135 source_addr16.insert(source_addr16.end(), packet.begin() + 13, packet.begin() + 15); //insert source address part of packet into new vector
sleighton 6:fb0316cafaa6 136 checkSourceAddr(source_addr16);
sleighton 3:cf539cfd3d59 137
sleighton 3:cf539cfd3d59 138 std::vector<uint8_t> data; //create new vector to store data
sleighton 3:cf539cfd3d59 139 data.insert(data.end(), packet.begin() + 15, packet.end() -1); //insert data part of packet into new vector
sleighton 7:c3acafdb70c0 140 /*for(int i = 0; i<data.size();i++){
sleighton 3:cf539cfd3d59 141 printf("Data: %d\n",(int)data[i]); //display data from packet
sleighton 7:c3acafdb70c0 142 }*/
sleighton 7:c3acafdb70c0 143 RxDataHandler(data);
sleighton 3:cf539cfd3d59 144
sleighton 3:cf539cfd3d59 145 break;
sleighton 3:cf539cfd3d59 146 }
sleighton 6:fb0316cafaa6 147 case 0x88:{ //AT response packet command
sleighton 6:fb0316cafaa6 148 if(packet[7] == 0x00){ //if packet command status is ok
sleighton 6:fb0316cafaa6 149 std::vector<uint8_t> data; //create new vector to store data
sleighton 6:fb0316cafaa6 150 data.insert(data.end(), packet.begin() + 8, packet.end() -1); //insert data part of packet into new vector
sleighton 7:c3acafdb70c0 151 if((packet[5] == 0x4D) & (packet[6] == 0x59)){ //if AT command is 'MY'
sleighton 7:c3acafdb70c0 152 checkSourceAddr(data); //call function to enter own network address in node_list
sleighton 6:fb0316cafaa6 153 }
sleighton 6:fb0316cafaa6 154 }
sleighton 6:fb0316cafaa6 155 break;
sleighton 6:fb0316cafaa6 156 }
sleighton 3:cf539cfd3d59 157 default:
sleighton 3:cf539cfd3d59 158 printf("Received API address not recognised: %c",command);
sleighton 3:cf539cfd3d59 159 }
sleighton 3:cf539cfd3d59 160 }
sleighton 3:cf539cfd3d59 161
sleighton 3:cf539cfd3d59 162 void XBee_Robot::checkSourceAddr(std::vector<uint8_t> & addr)
sleighton 3:cf539cfd3d59 163 {
sleighton 4:af08c7749f9d 164 bool exists = false;
sleighton 4:af08c7749f9d 165 for (int i = 0; i<node_list.size();i++){ //search each entry in node_list for matching address
sleighton 8:7e936dc02dec 166 currentIndex = i; //update currentIndex
sleighton 4:af08c7749f9d 167 if(node_list[i].getAddr() == addr){
sleighton 4:af08c7749f9d 168 exists = true;
sleighton 4:af08c7749f9d 169 printf("Recognised node %d\n",node_list[i].getIndex()); //print node number
sleighton 4:af08c7749f9d 170 }
sleighton 4:af08c7749f9d 171 }
sleighton 4:af08c7749f9d 172 if (exists == false){ //add new address to list if no match found
sleighton 8:7e936dc02dec 173 currentIndex++; //increment current index for new entry
sleighton 4:af08c7749f9d 174 NetworkNode newNode(addr,node_list.size());//create new node
sleighton 4:af08c7749f9d 175 node_list.push_back(newNode); //add new node to list
sleighton 6:fb0316cafaa6 176 printf("New address added: Node %d\n",(int)node_list.size()-1);
sleighton 3:cf539cfd3d59 177 }
sleighton 3:cf539cfd3d59 178
sleighton 4:af08c7749f9d 179 }
sleighton 7:c3acafdb70c0 180
sleighton 7:c3acafdb70c0 181 void XBee_Robot::RxDataHandler(std::vector<uint8_t> & packet)
sleighton 7:c3acafdb70c0 182 {
sleighton 7:c3acafdb70c0 183 uint8_t command = packet[0]; //take data command
sleighton 7:c3acafdb70c0 184 switch (command) { //index for different commands
sleighton 7:c3acafdb70c0 185 case 0xFF:{ //Receive proximity command
sleighton 8:7e936dc02dec 186 uint16_t prox = ((uint16_t)packet[1] << 8) | packet[2]; //create word to assemble upper and lower proximity data bytes
sleighton 8:7e936dc02dec 187 printf("Proximity: %d\n",(int)prox); //display data from packet
sleighton 8:7e936dc02dec 188 break;
sleighton 8:7e936dc02dec 189 }
sleighton 8:7e936dc02dec 190 case 0x01:{ //Receive location command
sleighton 8:7e936dc02dec 191 node_list[currentIndex].setCoordinates((int)packet[1],(int)packet[2]); //update coordinates for corresponding node
sleighton 8:7e936dc02dec 192 printf("X = %d, Y = %d\n",node_list[currentIndex].getX(),node_list[currentIndex].getY()); //display data from packet
sleighton 7:c3acafdb70c0 193 break;
sleighton 7:c3acafdb70c0 194 }
sleighton 7:c3acafdb70c0 195 default:
sleighton 7:c3acafdb70c0 196 printf("Received data command not recognised: %c",command);
sleighton 7:c3acafdb70c0 197 }
sleighton 7:c3acafdb70c0 198 }