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@12:a7ec1238373e, 2016-03-04 (annotated)

Committer:
sleighton
Date:
Fri Mar 04 18:46:05 2016 +0000
Revision:
12:a7ec1238373e
Parent:
11:6b699617fc7f 
Before Tidy up

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 11:6b699617fc7f 12 NetworkNode::NetworkNode(std::vector<uint8_t> & addrIn, std::vector<uint8_t> & addr64In, int indexIn){
sleighton 11:6b699617fc7f 13 addr = addrIn;
sleighton 11:6b699617fc7f 14 nodeNum = indexIn;
sleighton 11:6b699617fc7f 15 addr64 = addr64In;
sleighton 11:6b699617fc7f 16 }
sleighton 11:6b699617fc7f 17
sleighton 11:6b699617fc7f 18 std::vector<uint8_t> NetworkNode::getAddr(){ //returns 16 bit address of node
sleighton 4:af08c7749f9d 19 return addr;
sleighton 2:5040ec01dba1 20 }
sleighton 2:5040ec01dba1 21
sleighton 11:6b699617fc7f 22 std::vector<uint8_t> NetworkNode::getAddr64(){ //returns 64 bit address of node
sleighton 11:6b699617fc7f 23 return addr64;
sleighton 11:6b699617fc7f 24 }
sleighton 11:6b699617fc7f 25
sleighton 4:af08c7749f9d 26 int NetworkNode::getIndex(){ //returns index of node
sleighton 4:af08c7749f9d 27 return nodeNum;
sleighton 4:af08c7749f9d 28 }
sleighton 8:7e936dc02dec 29
sleighton 8:7e936dc02dec 30 int NetworkNode::getX(){
sleighton 8:7e936dc02dec 31 return x;
sleighton 8:7e936dc02dec 32 }
sleighton 8:7e936dc02dec 33
sleighton 8:7e936dc02dec 34 int NetworkNode::getY(){
sleighton 8:7e936dc02dec 35 return y;
sleighton 8:7e936dc02dec 36 }
sleighton 8:7e936dc02dec 37
sleighton 9:d5e7e772d5a4 38 int NetworkNode::getHeading(){
sleighton 9:d5e7e772d5a4 39 return heading;
sleighton 9:d5e7e772d5a4 40 }
sleighton 9:d5e7e772d5a4 41
sleighton 9:d5e7e772d5a4 42 void NetworkNode::setCoordinates(int x_in, int y_in, int heading_in){
sleighton 8:7e936dc02dec 43 x = x_in;
sleighton 8:7e936dc02dec 44 y = y_in;
sleighton 9:d5e7e772d5a4 45 heading = heading_in;
sleighton 8:7e936dc02dec 46 }
sleighton 4:af08c7749f9d 47
sleighton 4:af08c7749f9d 48 void NetworkNode::setIndex(int indexIn){ //sets index of node
sleighton 4:af08c7749f9d 49 nodeNum = indexIn;
sleighton 4:af08c7749f9d 50 }
sleighton 4:af08c7749f9d 51
sleighton 4:af08c7749f9d 52 void NetworkNode::setAddr(std::vector<uint8_t> & addrIn){ //sets address of node
sleighton 4:af08c7749f9d 53 addr = addrIn;
sleighton 4:af08c7749f9d 54 }
sleighton 4:af08c7749f9d 55
sleighton 6:fb0316cafaa6 56 /**************************************************************************************************/
sleighton 6:fb0316cafaa6 57
sleighton 4:af08c7749f9d 58 //XBEE ROBOT CLASS METHODS
sleighton 6:fb0316cafaa6 59 XBee_Robot::XBee_Robot(PinName _txIn, PinName _rxIn): dataLink(_txIn,_rxIn){
sleighton 12:a7ec1238373e 60 dataLink.attach(this,&XBee_Robot::Rx_interrupt, Serial::RxIrq); //set interrupt function on receive pin
sleighton 7:c3acafdb70c0 61 ATQuery(0x4D,0x59); //create AT query with AT command 'MY' to query own 16 bit network address
sleighton 10:cb1d91c06093 62 commandFlag = 0; //set command flag to 0 on initialisation
sleighton 12:a7ec1238373e 63 finishedFlag = 0; //set finished flag to 0 on intialisation
sleighton 2:5040ec01dba1 64 }
sleighton 2:5040ec01dba1 65
sleighton 2:5040ec01dba1 66 void XBee_Robot::Rx_interrupt()
sleighton 2:5040ec01dba1 67 {
sleighton 3:cf539cfd3d59 68 std::vector<uint8_t> Rx_buffer;
sleighton 2:5040ec01dba1 69 while(dataLink.readable()){
sleighton 3:cf539cfd3d59 70 Rx_buffer.push_back(dataLink.getc());//add each incoming byte to buffer
sleighton 6:fb0316cafaa6 71 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 72 }
sleighton 12:a7ec1238373e 73
sleighton 3:cf539cfd3d59 74 //Check valid packet delimeter and checksum
sleighton 3:cf539cfd3d59 75 if((Rx_buffer[0] == 0x7E) && (Rx_buffer[Rx_buffer.size()] == calculateChecksum(Rx_buffer)))
sleighton 3:cf539cfd3d59 76 RxPacketControl(Rx_buffer); //call packet control function
sleighton 12:a7ec1238373e 77 else
sleighton 12:a7ec1238373e 78 printf("Packet failed delimeter and checksum check");
sleighton 2:5040ec01dba1 79 }
sleighton 2:5040ec01dba1 80
sleighton 2:5040ec01dba1 81 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 82 {
sleighton 2:5040ec01dba1 83 //calculate checksum
sleighton 2:5040ec01dba1 84 uint16_t length = 0x0E + dataLength; //calculate length of packet (14 + data length)
sleighton 2:5040ec01dba1 85 uint8_t lengthu = length >>8; //upper 8 bits
sleighton 2:5040ec01dba1 86 uint8_t lengthl = length & 0xFF; //lower 8 bits
sleighton 2:5040ec01dba1 87
sleighton 2:5040ec01dba1 88
sleighton 2:5040ec01dba1 89 std::vector<uint8_t> transmitRequestPacket; //create new vector packet
sleighton 2:5040ec01dba1 90 //populate packet
sleighton 2:5040ec01dba1 91 transmitRequestPacket.push_back(0x7E); //start delimeter
sleighton 2:5040ec01dba1 92 transmitRequestPacket.push_back(lengthu); //upper byte of length
sleighton 2:5040ec01dba1 93 transmitRequestPacket.push_back(lengthl); //lower byte of length
sleighton 2:5040ec01dba1 94 transmitRequestPacket.push_back(0x10); //API ID (transmit request)
sleighton 2:5040ec01dba1 95 transmitRequestPacket.push_back(0x01); //channel ID
sleighton 2:5040ec01dba1 96 transmitRequestPacket.insert(transmitRequestPacket.end(), BitAddress64, BitAddress64+8); //64 bit destination address
sleighton 2:5040ec01dba1 97 transmitRequestPacket.insert(transmitRequestPacket.end(), BitAddress16, BitAddress16+2); //16 bit network address
sleighton 2:5040ec01dba1 98 transmitRequestPacket.push_back(broadcastRadius); //broadcast radius (0 = max hops)
sleighton 2:5040ec01dba1 99 transmitRequestPacket.push_back(options); //additional options for packet
sleighton 2:5040ec01dba1 100 transmitRequestPacket.insert(transmitRequestPacket.end(), data, data+dataLength); //data
sleighton 2:5040ec01dba1 101 uint8_t checksum = calculateChecksum(transmitRequestPacket);
sleighton 2:5040ec01dba1 102 transmitRequestPacket.push_back(checksum); //calculate and add checksum
sleighton 2:5040ec01dba1 103
sleighton 2:5040ec01dba1 104 for (int i = 0; i < transmitRequestPacket.size(); i++){
sleighton 2:5040ec01dba1 105 dataLink.printf("%c",transmitRequestPacket[i]); //send packet
sleighton 2:5040ec01dba1 106 }
sleighton 2:5040ec01dba1 107 }
sleighton 2:5040ec01dba1 108
sleighton 6:fb0316cafaa6 109 void XBee_Robot::ATQuery(uint8_t ATu, uint8_t ATl)
sleighton 6:fb0316cafaa6 110 {
sleighton 6:fb0316cafaa6 111 //calculate checksum
sleighton 6:fb0316cafaa6 112 uint8_t lengthu = 0; //upper 8 bits of length
sleighton 6:fb0316cafaa6 113 uint8_t lengthl = 0x04; //lower 8 bits of length
sleighton 6:fb0316cafaa6 114
sleighton 6:fb0316cafaa6 115
sleighton 6:fb0316cafaa6 116 std::vector<uint8_t> ATRequestPacket; //create new vector packet
sleighton 6:fb0316cafaa6 117 //populate packet
sleighton 6:fb0316cafaa6 118 ATRequestPacket.push_back(0x7E); //start delimeter
sleighton 6:fb0316cafaa6 119 ATRequestPacket.push_back(lengthu); //upper byte of length
sleighton 6:fb0316cafaa6 120 ATRequestPacket.push_back(lengthl); //lower byte of length
sleighton 6:fb0316cafaa6 121 ATRequestPacket.push_back(0x08); //API ID (AT request)
sleighton 6:fb0316cafaa6 122 ATRequestPacket.push_back(0x52); //channel ID
sleighton 6:fb0316cafaa6 123 ATRequestPacket.push_back(ATu); //AT command (upper byte)
sleighton 6:fb0316cafaa6 124 ATRequestPacket.push_back(ATl); //AT command (lower byte)
sleighton 6:fb0316cafaa6 125 uint8_t checksum = calculateChecksum(ATRequestPacket);
sleighton 6:fb0316cafaa6 126 ATRequestPacket.push_back(checksum); //calculate and add checksum
sleighton 6:fb0316cafaa6 127
sleighton 6:fb0316cafaa6 128 for (int i = 0; i < ATRequestPacket.size(); i++){
sleighton 6:fb0316cafaa6 129 dataLink.printf("%c",ATRequestPacket[i]); //send packet
sleighton 6:fb0316cafaa6 130 }
sleighton 6:fb0316cafaa6 131 }
sleighton 6:fb0316cafaa6 132
sleighton 2:5040ec01dba1 133 uint8_t XBee_Robot::calculateChecksum(std::vector<uint8_t> & packet)
sleighton 2:5040ec01dba1 134 {
sleighton 2:5040ec01dba1 135 uint8_t checksum = 0xFF; //start with FF as last byte of sum is subtracted from FF
sleighton 2:5040ec01dba1 136 for (int i = 3; i < packet.size(); i++)
sleighton 2:5040ec01dba1 137 checksum -= packet[i];
sleighton 2:5040ec01dba1 138 return checksum;
sleighton 2:5040ec01dba1 139
sleighton 3:cf539cfd3d59 140 }
sleighton 3:cf539cfd3d59 141
sleighton 3:cf539cfd3d59 142 void XBee_Robot::RxPacketControl(std::vector<uint8_t> & packet)
sleighton 6:fb0316cafaa6 143 {
sleighton 3:cf539cfd3d59 144 uint8_t command = packet[3]; //take API address
sleighton 3:cf539cfd3d59 145 switch (command) { //index for different commands
sleighton 3:cf539cfd3d59 146 case 0x90:{ //Receive packet command
sleighton 3:cf539cfd3d59 147
sleighton 11:6b699617fc7f 148 std::vector<uint8_t> source_addr16; //create new vector to 16 bit store source address
sleighton 11:6b699617fc7f 149 std::vector<uint8_t> source_addr64; //create new vector to 64 bit store source address
sleighton 11:6b699617fc7f 150 source_addr16.insert(source_addr16.end(), packet.begin() + 12, packet.begin() + 14); //insert source address part of packet into new vector
sleighton 11:6b699617fc7f 151 source_addr64.insert(source_addr64.end(), packet.begin() + 4, packet.begin() + 12); //insert source address part of packet into new vector
sleighton 11:6b699617fc7f 152 checkSourceAddr(source_addr16,source_addr64);
sleighton 3:cf539cfd3d59 153
sleighton 3:cf539cfd3d59 154 std::vector<uint8_t> data; //create new vector to store data
sleighton 3:cf539cfd3d59 155 data.insert(data.end(), packet.begin() + 15, packet.end() -1); //insert data part of packet into new vector
sleighton 7:c3acafdb70c0 156 /*for(int i = 0; i<data.size();i++){
sleighton 3:cf539cfd3d59 157 printf("Data: %d\n",(int)data[i]); //display data from packet
sleighton 7:c3acafdb70c0 158 }*/
sleighton 7:c3acafdb70c0 159 RxDataHandler(data);
sleighton 3:cf539cfd3d59 160
sleighton 3:cf539cfd3d59 161 break;
sleighton 3:cf539cfd3d59 162 }
sleighton 6:fb0316cafaa6 163 case 0x88:{ //AT response packet command
sleighton 6:fb0316cafaa6 164 if(packet[7] == 0x00){ //if packet command status is ok
sleighton 6:fb0316cafaa6 165 std::vector<uint8_t> data; //create new vector to store data
sleighton 6:fb0316cafaa6 166 data.insert(data.end(), packet.begin() + 8, packet.end() -1); //insert data part of packet into new vector
sleighton 7:c3acafdb70c0 167 if((packet[5] == 0x4D) & (packet[6] == 0x59)){ //if AT command is 'MY'
sleighton 7:c3acafdb70c0 168 checkSourceAddr(data); //call function to enter own network address in node_list
sleighton 6:fb0316cafaa6 169 }
sleighton 6:fb0316cafaa6 170 }
sleighton 6:fb0316cafaa6 171 break;
sleighton 6:fb0316cafaa6 172 }
sleighton 11:6b699617fc7f 173 case 0x8B: { //Tx status packet command
sleighton 11:6b699617fc7f 174 if(packet[8] == 0 )//if delivery status is 00 (success)
sleighton 11:6b699617fc7f 175 printf("Packet successfully transmitted to 16 bit destination address: %c%c",packet[5],packet[6]);
sleighton 11:6b699617fc7f 176 else
sleighton 11:6b699617fc7f 177 printf("Packet delivery failed (status %c)",packet[8]);
sleighton 11:6b699617fc7f 178 break;
sleighton 11:6b699617fc7f 179 }
sleighton 3:cf539cfd3d59 180 default:
sleighton 3:cf539cfd3d59 181 printf("Received API address not recognised: %c",command);
sleighton 3:cf539cfd3d59 182 }
sleighton 3:cf539cfd3d59 183 }
sleighton 3:cf539cfd3d59 184
sleighton 11:6b699617fc7f 185 void XBee_Robot::checkSourceAddr(std::vector<uint8_t> & addr, std::vector<uint8_t> & addr64)
sleighton 11:6b699617fc7f 186 {
sleighton 11:6b699617fc7f 187 bool exists = false;
sleighton 11:6b699617fc7f 188 for (int i = 0; i<node_list.size();i++){ //search each entry in node_list for matching address
sleighton 11:6b699617fc7f 189 currentIndex = i; //update currentIndex
sleighton 11:6b699617fc7f 190 if(node_list[i].getAddr() == addr){
sleighton 11:6b699617fc7f 191 exists = true;
sleighton 11:6b699617fc7f 192 printf("Recognised node %d\n",node_list[i].getIndex()); //print node number
sleighton 11:6b699617fc7f 193 }
sleighton 11:6b699617fc7f 194 }
sleighton 11:6b699617fc7f 195 if (exists == false){ //add new address to list if no match found
sleighton 11:6b699617fc7f 196 currentIndex++; //increment current index for new entry
sleighton 11:6b699617fc7f 197 NetworkNode newNode(addr,addr64,node_list.size());//create new node
sleighton 11:6b699617fc7f 198 node_list.push_back(newNode); //add new node to list
sleighton 11:6b699617fc7f 199 printf("New address added: Node %d\n",(int)node_list.size()-1);
sleighton 11:6b699617fc7f 200 }
sleighton 11:6b699617fc7f 201
sleighton 11:6b699617fc7f 202 }
sleighton 11:6b699617fc7f 203
sleighton 3:cf539cfd3d59 204 void XBee_Robot::checkSourceAddr(std::vector<uint8_t> & addr)
sleighton 3:cf539cfd3d59 205 {
sleighton 4:af08c7749f9d 206 bool exists = false;
sleighton 4:af08c7749f9d 207 for (int i = 0; i<node_list.size();i++){ //search each entry in node_list for matching address
sleighton 8:7e936dc02dec 208 currentIndex = i; //update currentIndex
sleighton 4:af08c7749f9d 209 if(node_list[i].getAddr() == addr){
sleighton 4:af08c7749f9d 210 exists = true;
sleighton 4:af08c7749f9d 211 printf("Recognised node %d\n",node_list[i].getIndex()); //print node number
sleighton 4:af08c7749f9d 212 }
sleighton 4:af08c7749f9d 213 }
sleighton 4:af08c7749f9d 214 if (exists == false){ //add new address to list if no match found
sleighton 8:7e936dc02dec 215 currentIndex++; //increment current index for new entry
sleighton 11:6b699617fc7f 216 NetworkNode newNode(addr,node_list.size()); //create new node
sleighton 4:af08c7749f9d 217 node_list.push_back(newNode); //add new node to list
sleighton 6:fb0316cafaa6 218 printf("New address added: Node %d\n",(int)node_list.size()-1);
sleighton 3:cf539cfd3d59 219 }
sleighton 3:cf539cfd3d59 220
sleighton 4:af08c7749f9d 221 }
sleighton 7:c3acafdb70c0 222
sleighton 7:c3acafdb70c0 223 void XBee_Robot::RxDataHandler(std::vector<uint8_t> & packet)
sleighton 7:c3acafdb70c0 224 {
sleighton 7:c3acafdb70c0 225 uint8_t command = packet[0]; //take data command
sleighton 7:c3acafdb70c0 226 switch (command) { //index for different commands
sleighton 7:c3acafdb70c0 227 case 0xFF:{ //Receive proximity command
sleighton 8:7e936dc02dec 228 uint16_t prox = ((uint16_t)packet[1] << 8) | packet[2]; //create word to assemble upper and lower proximity data bytes
sleighton 8:7e936dc02dec 229 printf("Proximity: %d\n",(int)prox); //display data from packet
sleighton 8:7e936dc02dec 230 break;
sleighton 8:7e936dc02dec 231 }
sleighton 8:7e936dc02dec 232 case 0x01:{ //Receive location command
sleighton 12:a7ec1238373e 233 node_list[currentIndex].setCoordinates(convSigned((int)packet[1]),convSigned((int)packet[2]),convSigned((int)packet[3])); //update coordinates for corresponding node
sleighton 12:a7ec1238373e 234 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
sleighton 7:c3acafdb70c0 235 break;
sleighton 7:c3acafdb70c0 236 }
sleighton 10:cb1d91c06093 237 case 0x02: { //Receive destination coordinates from coordinator command
sleighton 10:cb1d91c06093 238 uint16_t comX = ((uint16_t)packet[1] << 8) | packet[2]; //create word to assemble upper and lower byte of x coordinate
sleighton 10:cb1d91c06093 239 commandX = int(comX); //set commandX to received coordinate
sleighton 10:cb1d91c06093 240 uint16_t comY = ((uint16_t)packet[3] << 8) | packet[4];
sleighton 10:cb1d91c06093 241 commandY = int(comY);
sleighton 10:cb1d91c06093 242 commandFlag = 1; //set command flag to indicate received coordinates
sleighton 10:cb1d91c06093 243 break;
sleighton 10:cb1d91c06093 244 }
sleighton 12:a7ec1238373e 245 case 0xFA: { //Receive router robot finished command
sleighton 12:a7ec1238373e 246 printf("Finished flag set\n");
sleighton 12:a7ec1238373e 247 finishedFlag = 1; //set command flag to indicate received coordinates
sleighton 12:a7ec1238373e 248 break;
sleighton 12:a7ec1238373e 249 }
sleighton 12:a7ec1238373e 250 case 0xFC: { //Receive obstacle coordinate command
sleighton 12:a7ec1238373e 251 printf("Obstacle coordinate received at %d, %d\n",convSigned((int)packet[1]),convSigned((int)packet[2]));
sleighton 12:a7ec1238373e 252 std::vector <int> coordinate;
sleighton 12:a7ec1238373e 253 coordinate.push_back(convSigned((int)packet[1]));
sleighton 12:a7ec1238373e 254 coordinate.push_back(convSigned((int)packet[2]));
sleighton 12:a7ec1238373e 255 obstacles.push_back(coordinate);
sleighton 12:a7ec1238373e 256 break;
sleighton 12:a7ec1238373e 257 }
sleighton 7:c3acafdb70c0 258 default:
sleighton 7:c3acafdb70c0 259 printf("Received data command not recognised: %c",command);
sleighton 7:c3acafdb70c0 260 }
sleighton 10:cb1d91c06093 261 }
sleighton 10:cb1d91c06093 262
sleighton 10:cb1d91c06093 263 int XBee_Robot::getCommandX(){
sleighton 10:cb1d91c06093 264 return commandX;
sleighton 10:cb1d91c06093 265 }
sleighton 10:cb1d91c06093 266
sleighton 10:cb1d91c06093 267 int XBee_Robot::getCommandY(){
sleighton 10:cb1d91c06093 268 return commandY;
sleighton 10:cb1d91c06093 269 }
sleighton 10:cb1d91c06093 270
sleighton 10:cb1d91c06093 271 int XBee_Robot::getCommandFlag(){
sleighton 10:cb1d91c06093 272 return commandFlag; //return value of command flag
sleighton 10:cb1d91c06093 273 }
sleighton 10:cb1d91c06093 274
sleighton 12:a7ec1238373e 275 int XBee_Robot::getFinishedFlag(){
sleighton 12:a7ec1238373e 276 return finishedFlag; //return value of finished flag
sleighton 12:a7ec1238373e 277 }
sleighton 12:a7ec1238373e 278
sleighton 12:a7ec1238373e 279 void XBee_Robot::resetFinishedFlag(){
sleighton 12:a7ec1238373e 280 finishedFlag = 0; //reset finished flag
sleighton 12:a7ec1238373e 281 }
sleighton 12:a7ec1238373e 282
sleighton 10:cb1d91c06093 283 void XBee_Robot::resetCommandFlag(){
sleighton 10:cb1d91c06093 284 commandFlag = 0; //reset command flag
sleighton 12:a7ec1238373e 285 }
sleighton 12:a7ec1238373e 286
sleighton 12:a7ec1238373e 287 int XBee_Robot::checkObstacle(int x_in, int y_in){
sleighton 12:a7ec1238373e 288 for (int i = 0; i<obstacles.size();i++){ //search each entry in node_list for matching node
sleighton 12:a7ec1238373e 289 if((obstacles[i][0] == x_in)&&(obstacles[i][1] == y_in)){
sleighton 12:a7ec1238373e 290 return 1; //return 1 if obstacle exists at given coordinates
sleighton 12:a7ec1238373e 291 printf("Obstacle found at %d,%d\n",obstacles[i][0],obstacles[i][1]); //print node number
sleighton 12:a7ec1238373e 292 }
sleighton 12:a7ec1238373e 293 }
sleighton 12:a7ec1238373e 294 return 0; //return 0 if no obstacle exists at given coordinates
sleighton 12:a7ec1238373e 295 }
sleighton 12:a7ec1238373e 296
sleighton 12:a7ec1238373e 297 int XBee_Robot::convSigned(int int_in){
sleighton 12:a7ec1238373e 298 if(int_in >= 128)
sleighton 12:a7ec1238373e 299 return int_in-256; //if value is greater or equal to 128 convert to negative number
sleighton 12:a7ec1238373e 300 return int_in;
sleighton 7:c3acafdb70c0 301 }