Sam Leighton / XBee_Robot

XBee_Robot.cpp@7:c3acafdb70c0, 2016-01-11 (annotated)

Committer:
sleighton
Date:
Mon Jan 11 11:49:37 2016 +0000
Revision:
7:c3acafdb70c0
Parent:
6:fb0316cafaa6
Child:
8:7e936dc02dec
Updated with RxDataHandler which handles received data according to its type eg proximity reading

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