Sam Leighton / XBee_Robot

XBee_Robot.cpp@9:d5e7e772d5a4, 2016-01-31 (annotated)

Committer:
sleighton
Date:
Sun Jan 31 19:34:21 2016 +0000
Revision:
9:d5e7e772d5a4
Parent:
8:7e936dc02dec
Child:
10:cb1d91c06093
Tracks heading

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