| User | Revision | Line number | New 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 |
7:c3acafdb70c0
|
60
|
ATQuery(0x4D,0x59); //create AT query with AT command 'MY' to query own 16 bit network address
|
| sleighton |
10:cb1d91c06093
|
61
|
commandFlag = 0; //set command flag to 0 on initialisation
|
| sleighton |
6:fb0316cafaa6
|
62
|
}
|
| sleighton |
4:af08c7749f9d
|
63
|
|
| sleighton |
2:5040ec01dba1
|
64
|
void XBee_Robot::setRxInterrupt()
|
| sleighton |
2:5040ec01dba1
|
65
|
{
|
| sleighton |
2:5040ec01dba1
|
66
|
dataLink.attach(this,&XBee_Robot::Rx_interrupt, Serial::RxIrq);
|
| sleighton |
2:5040ec01dba1
|
67
|
}
|
| sleighton |
2:5040ec01dba1
|
68
|
|
| sleighton |
2:5040ec01dba1
|
69
|
void XBee_Robot::Rx_interrupt()
|
| sleighton |
2:5040ec01dba1
|
70
|
{
|
| sleighton |
3:cf539cfd3d59
|
71
|
std::vector<uint8_t> Rx_buffer;
|
| sleighton |
2:5040ec01dba1
|
72
|
while(dataLink.readable()){
|
| sleighton |
3:cf539cfd3d59
|
73
|
Rx_buffer.push_back(dataLink.getc());//add each incoming byte to buffer
|
| sleighton |
6:fb0316cafaa6
|
74
|
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
|
75
|
}
|
| sleighton |
3:cf539cfd3d59
|
76
|
|
| sleighton |
3:cf539cfd3d59
|
77
|
//Check valid packet delimeter and checksum
|
| sleighton |
3:cf539cfd3d59
|
78
|
if((Rx_buffer[0] == 0x7E) && (Rx_buffer[Rx_buffer.size()] == calculateChecksum(Rx_buffer)))
|
| sleighton |
3:cf539cfd3d59
|
79
|
RxPacketControl(Rx_buffer); //call packet control function
|
| sleighton |
2:5040ec01dba1
|
80
|
}
|
| sleighton |
2:5040ec01dba1
|
81
|
|
| sleighton |
2:5040ec01dba1
|
82
|
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
|
83
|
{
|
| sleighton |
2:5040ec01dba1
|
84
|
//calculate checksum
|
| sleighton |
2:5040ec01dba1
|
85
|
uint16_t length = 0x0E + dataLength; //calculate length of packet (14 + data length)
|
| sleighton |
2:5040ec01dba1
|
86
|
uint8_t lengthu = length >>8; //upper 8 bits
|
| sleighton |
2:5040ec01dba1
|
87
|
uint8_t lengthl = length & 0xFF; //lower 8 bits
|
| sleighton |
2:5040ec01dba1
|
88
|
|
| sleighton |
2:5040ec01dba1
|
89
|
|
| sleighton |
2:5040ec01dba1
|
90
|
std::vector<uint8_t> transmitRequestPacket; //create new vector packet
|
| sleighton |
2:5040ec01dba1
|
91
|
//populate packet
|
| sleighton |
2:5040ec01dba1
|
92
|
transmitRequestPacket.push_back(0x7E); //start delimeter
|
| sleighton |
2:5040ec01dba1
|
93
|
transmitRequestPacket.push_back(lengthu); //upper byte of length
|
| sleighton |
2:5040ec01dba1
|
94
|
transmitRequestPacket.push_back(lengthl); //lower byte of length
|
| sleighton |
2:5040ec01dba1
|
95
|
transmitRequestPacket.push_back(0x10); //API ID (transmit request)
|
| sleighton |
2:5040ec01dba1
|
96
|
transmitRequestPacket.push_back(0x01); //channel ID
|
| sleighton |
2:5040ec01dba1
|
97
|
transmitRequestPacket.insert(transmitRequestPacket.end(), BitAddress64, BitAddress64+8); //64 bit destination address
|
| sleighton |
2:5040ec01dba1
|
98
|
transmitRequestPacket.insert(transmitRequestPacket.end(), BitAddress16, BitAddress16+2); //16 bit network address
|
| sleighton |
2:5040ec01dba1
|
99
|
transmitRequestPacket.push_back(broadcastRadius); //broadcast radius (0 = max hops)
|
| sleighton |
2:5040ec01dba1
|
100
|
transmitRequestPacket.push_back(options); //additional options for packet
|
| sleighton |
2:5040ec01dba1
|
101
|
transmitRequestPacket.insert(transmitRequestPacket.end(), data, data+dataLength); //data
|
| sleighton |
2:5040ec01dba1
|
102
|
uint8_t checksum = calculateChecksum(transmitRequestPacket);
|
| sleighton |
2:5040ec01dba1
|
103
|
transmitRequestPacket.push_back(checksum); //calculate and add checksum
|
| sleighton |
2:5040ec01dba1
|
104
|
|
| sleighton |
2:5040ec01dba1
|
105
|
for (int i = 0; i < transmitRequestPacket.size(); i++){
|
| sleighton |
2:5040ec01dba1
|
106
|
dataLink.printf("%c",transmitRequestPacket[i]); //send packet
|
| sleighton |
2:5040ec01dba1
|
107
|
}
|
| sleighton |
2:5040ec01dba1
|
108
|
}
|
| sleighton |
2:5040ec01dba1
|
109
|
|
| sleighton |
6:fb0316cafaa6
|
110
|
void XBee_Robot::ATQuery(uint8_t ATu, uint8_t ATl)
|
| sleighton |
6:fb0316cafaa6
|
111
|
{
|
| sleighton |
6:fb0316cafaa6
|
112
|
//calculate checksum
|
| sleighton |
6:fb0316cafaa6
|
113
|
uint8_t lengthu = 0; //upper 8 bits of length
|
| sleighton |
6:fb0316cafaa6
|
114
|
uint8_t lengthl = 0x04; //lower 8 bits of length
|
| sleighton |
6:fb0316cafaa6
|
115
|
|
| sleighton |
6:fb0316cafaa6
|
116
|
|
| sleighton |
6:fb0316cafaa6
|
117
|
std::vector<uint8_t> ATRequestPacket; //create new vector packet
|
| sleighton |
6:fb0316cafaa6
|
118
|
//populate packet
|
| sleighton |
6:fb0316cafaa6
|
119
|
ATRequestPacket.push_back(0x7E); //start delimeter
|
| sleighton |
6:fb0316cafaa6
|
120
|
ATRequestPacket.push_back(lengthu); //upper byte of length
|
| sleighton |
6:fb0316cafaa6
|
121
|
ATRequestPacket.push_back(lengthl); //lower byte of length
|
| sleighton |
6:fb0316cafaa6
|
122
|
ATRequestPacket.push_back(0x08); //API ID (AT request)
|
| sleighton |
6:fb0316cafaa6
|
123
|
ATRequestPacket.push_back(0x52); //channel ID
|
| sleighton |
6:fb0316cafaa6
|
124
|
ATRequestPacket.push_back(ATu); //AT command (upper byte)
|
| sleighton |
6:fb0316cafaa6
|
125
|
ATRequestPacket.push_back(ATl); //AT command (lower byte)
|
| sleighton |
6:fb0316cafaa6
|
126
|
uint8_t checksum = calculateChecksum(ATRequestPacket);
|
| sleighton |
6:fb0316cafaa6
|
127
|
ATRequestPacket.push_back(checksum); //calculate and add checksum
|
| sleighton |
6:fb0316cafaa6
|
128
|
|
| sleighton |
6:fb0316cafaa6
|
129
|
for (int i = 0; i < ATRequestPacket.size(); i++){
|
| sleighton |
6:fb0316cafaa6
|
130
|
dataLink.printf("%c",ATRequestPacket[i]); //send packet
|
| sleighton |
6:fb0316cafaa6
|
131
|
}
|
| sleighton |
6:fb0316cafaa6
|
132
|
}
|
| sleighton |
6:fb0316cafaa6
|
133
|
|
| sleighton |
2:5040ec01dba1
|
134
|
uint8_t XBee_Robot::calculateChecksum(std::vector<uint8_t> & packet)
|
| sleighton |
2:5040ec01dba1
|
135
|
{
|
| sleighton |
2:5040ec01dba1
|
136
|
uint8_t checksum = 0xFF; //start with FF as last byte of sum is subtracted from FF
|
| sleighton |
2:5040ec01dba1
|
137
|
for (int i = 3; i < packet.size(); i++)
|
| sleighton |
2:5040ec01dba1
|
138
|
checksum -= packet[i];
|
| sleighton |
2:5040ec01dba1
|
139
|
return checksum;
|
| sleighton |
2:5040ec01dba1
|
140
|
|
| sleighton |
3:cf539cfd3d59
|
141
|
}
|
| sleighton |
3:cf539cfd3d59
|
142
|
|
| sleighton |
3:cf539cfd3d59
|
143
|
void XBee_Robot::RxPacketControl(std::vector<uint8_t> & packet)
|
| sleighton |
6:fb0316cafaa6
|
144
|
{
|
| sleighton |
3:cf539cfd3d59
|
145
|
uint8_t command = packet[3]; //take API address
|
| sleighton |
3:cf539cfd3d59
|
146
|
switch (command) { //index for different commands
|
| sleighton |
3:cf539cfd3d59
|
147
|
case 0x90:{ //Receive packet command
|
| sleighton |
3:cf539cfd3d59
|
148
|
|
| sleighton |
11:6b699617fc7f
|
149
|
std::vector<uint8_t> source_addr16; //create new vector to 16 bit store source address
|
| sleighton |
11:6b699617fc7f
|
150
|
std::vector<uint8_t> source_addr64; //create new vector to 64 bit store source address
|
| sleighton |
11:6b699617fc7f
|
151
|
source_addr16.insert(source_addr16.end(), packet.begin() + 12, packet.begin() + 14); //insert source address part of packet into new vector
|
| sleighton |
11:6b699617fc7f
|
152
|
source_addr64.insert(source_addr64.end(), packet.begin() + 4, packet.begin() + 12); //insert source address part of packet into new vector
|
| sleighton |
11:6b699617fc7f
|
153
|
checkSourceAddr(source_addr16,source_addr64);
|
| sleighton |
3:cf539cfd3d59
|
154
|
|
| sleighton |
3:cf539cfd3d59
|
155
|
std::vector<uint8_t> data; //create new vector to store data
|
| sleighton |
3:cf539cfd3d59
|
156
|
data.insert(data.end(), packet.begin() + 15, packet.end() -1); //insert data part of packet into new vector
|
| sleighton |
7:c3acafdb70c0
|
157
|
/*for(int i = 0; i<data.size();i++){
|
| sleighton |
3:cf539cfd3d59
|
158
|
printf("Data: %d\n",(int)data[i]); //display data from packet
|
| sleighton |
7:c3acafdb70c0
|
159
|
}*/
|
| sleighton |
7:c3acafdb70c0
|
160
|
RxDataHandler(data);
|
| sleighton |
3:cf539cfd3d59
|
161
|
|
| sleighton |
3:cf539cfd3d59
|
162
|
break;
|
| sleighton |
3:cf539cfd3d59
|
163
|
}
|
| sleighton |
6:fb0316cafaa6
|
164
|
case 0x88:{ //AT response packet command
|
| sleighton |
6:fb0316cafaa6
|
165
|
if(packet[7] == 0x00){ //if packet command status is ok
|
| sleighton |
6:fb0316cafaa6
|
166
|
std::vector<uint8_t> data; //create new vector to store data
|
| sleighton |
6:fb0316cafaa6
|
167
|
data.insert(data.end(), packet.begin() + 8, packet.end() -1); //insert data part of packet into new vector
|
| sleighton |
7:c3acafdb70c0
|
168
|
if((packet[5] == 0x4D) & (packet[6] == 0x59)){ //if AT command is 'MY'
|
| sleighton |
7:c3acafdb70c0
|
169
|
checkSourceAddr(data); //call function to enter own network address in node_list
|
| sleighton |
6:fb0316cafaa6
|
170
|
}
|
| sleighton |
6:fb0316cafaa6
|
171
|
}
|
| sleighton |
6:fb0316cafaa6
|
172
|
break;
|
| sleighton |
6:fb0316cafaa6
|
173
|
}
|
| sleighton |
11:6b699617fc7f
|
174
|
case 0x8B: { //Tx status packet command
|
| sleighton |
11:6b699617fc7f
|
175
|
if(packet[8] == 0 )//if delivery status is 00 (success)
|
| sleighton |
11:6b699617fc7f
|
176
|
printf("Packet successfully transmitted to 16 bit destination address: %c%c",packet[5],packet[6]);
|
| sleighton |
11:6b699617fc7f
|
177
|
else
|
| sleighton |
11:6b699617fc7f
|
178
|
printf("Packet delivery failed (status %c)",packet[8]);
|
| sleighton |
11:6b699617fc7f
|
179
|
break;
|
| sleighton |
11:6b699617fc7f
|
180
|
}
|
| sleighton |
3:cf539cfd3d59
|
181
|
default:
|
| sleighton |
3:cf539cfd3d59
|
182
|
printf("Received API address not recognised: %c",command);
|
| sleighton |
3:cf539cfd3d59
|
183
|
}
|
| sleighton |
3:cf539cfd3d59
|
184
|
}
|
| sleighton |
3:cf539cfd3d59
|
185
|
|
| sleighton |
11:6b699617fc7f
|
186
|
void XBee_Robot::checkSourceAddr(std::vector<uint8_t> & addr, std::vector<uint8_t> & addr64)
|
| sleighton |
11:6b699617fc7f
|
187
|
{
|
| sleighton |
11:6b699617fc7f
|
188
|
bool exists = false;
|
| sleighton |
11:6b699617fc7f
|
189
|
for (int i = 0; i<node_list.size();i++){ //search each entry in node_list for matching address
|
| sleighton |
11:6b699617fc7f
|
190
|
currentIndex = i; //update currentIndex
|
| sleighton |
11:6b699617fc7f
|
191
|
if(node_list[i].getAddr() == addr){
|
| sleighton |
11:6b699617fc7f
|
192
|
exists = true;
|
| sleighton |
11:6b699617fc7f
|
193
|
printf("Recognised node %d\n",node_list[i].getIndex()); //print node number
|
| sleighton |
11:6b699617fc7f
|
194
|
}
|
| sleighton |
11:6b699617fc7f
|
195
|
}
|
| sleighton |
11:6b699617fc7f
|
196
|
if (exists == false){ //add new address to list if no match found
|
| sleighton |
11:6b699617fc7f
|
197
|
currentIndex++; //increment current index for new entry
|
| sleighton |
11:6b699617fc7f
|
198
|
NetworkNode newNode(addr,addr64,node_list.size());//create new node
|
| sleighton |
11:6b699617fc7f
|
199
|
node_list.push_back(newNode); //add new node to list
|
| sleighton |
11:6b699617fc7f
|
200
|
printf("New address added: Node %d\n",(int)node_list.size()-1);
|
| sleighton |
11:6b699617fc7f
|
201
|
}
|
| sleighton |
11:6b699617fc7f
|
202
|
|
| sleighton |
11:6b699617fc7f
|
203
|
}
|
| sleighton |
11:6b699617fc7f
|
204
|
|
| sleighton |
3:cf539cfd3d59
|
205
|
void XBee_Robot::checkSourceAddr(std::vector<uint8_t> & addr)
|
| sleighton |
3:cf539cfd3d59
|
206
|
{
|
| sleighton |
4:af08c7749f9d
|
207
|
bool exists = false;
|
| sleighton |
4:af08c7749f9d
|
208
|
for (int i = 0; i<node_list.size();i++){ //search each entry in node_list for matching address
|
| sleighton |
8:7e936dc02dec
|
209
|
currentIndex = i; //update currentIndex
|
| sleighton |
4:af08c7749f9d
|
210
|
if(node_list[i].getAddr() == addr){
|
| sleighton |
4:af08c7749f9d
|
211
|
exists = true;
|
| sleighton |
4:af08c7749f9d
|
212
|
printf("Recognised node %d\n",node_list[i].getIndex()); //print node number
|
| sleighton |
4:af08c7749f9d
|
213
|
}
|
| sleighton |
4:af08c7749f9d
|
214
|
}
|
| sleighton |
4:af08c7749f9d
|
215
|
if (exists == false){ //add new address to list if no match found
|
| sleighton |
8:7e936dc02dec
|
216
|
currentIndex++; //increment current index for new entry
|
| sleighton |
11:6b699617fc7f
|
217
|
NetworkNode newNode(addr,node_list.size()); //create new node
|
| sleighton |
4:af08c7749f9d
|
218
|
node_list.push_back(newNode); //add new node to list
|
| sleighton |
6:fb0316cafaa6
|
219
|
printf("New address added: Node %d\n",(int)node_list.size()-1);
|
| sleighton |
3:cf539cfd3d59
|
220
|
}
|
| sleighton |
3:cf539cfd3d59
|
221
|
|
| sleighton |
4:af08c7749f9d
|
222
|
}
|
| sleighton |
7:c3acafdb70c0
|
223
|
|
| sleighton |
7:c3acafdb70c0
|
224
|
void XBee_Robot::RxDataHandler(std::vector<uint8_t> & packet)
|
| sleighton |
7:c3acafdb70c0
|
225
|
{
|
| sleighton |
7:c3acafdb70c0
|
226
|
uint8_t command = packet[0]; //take data command
|
| sleighton |
7:c3acafdb70c0
|
227
|
switch (command) { //index for different commands
|
| sleighton |
7:c3acafdb70c0
|
228
|
case 0xFF:{ //Receive proximity command
|
| sleighton |
8:7e936dc02dec
|
229
|
uint16_t prox = ((uint16_t)packet[1] << 8) | packet[2]; //create word to assemble upper and lower proximity data bytes
|
| sleighton |
8:7e936dc02dec
|
230
|
printf("Proximity: %d\n",(int)prox); //display data from packet
|
| sleighton |
8:7e936dc02dec
|
231
|
break;
|
| sleighton |
8:7e936dc02dec
|
232
|
}
|
| sleighton |
8:7e936dc02dec
|
233
|
case 0x01:{ //Receive location command
|
| sleighton |
9:d5e7e772d5a4
|
234
|
node_list[currentIndex].setCoordinates((int)packet[1],(int)packet[2],(int)packet[3]); //update coordinates for corresponding node
|
| sleighton |
9:d5e7e772d5a4
|
235
|
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
|
236
|
break;
|
| sleighton |
7:c3acafdb70c0
|
237
|
}
|
| sleighton |
10:cb1d91c06093
|
238
|
case 0x02: { //Receive destination coordinates from coordinator command
|
| sleighton |
10:cb1d91c06093
|
239
|
uint16_t comX = ((uint16_t)packet[1] << 8) | packet[2]; //create word to assemble upper and lower byte of x coordinate
|
| sleighton |
10:cb1d91c06093
|
240
|
commandX = int(comX); //set commandX to received coordinate
|
| sleighton |
10:cb1d91c06093
|
241
|
uint16_t comY = ((uint16_t)packet[3] << 8) | packet[4];
|
| sleighton |
10:cb1d91c06093
|
242
|
commandY = int(comY);
|
| sleighton |
10:cb1d91c06093
|
243
|
commandFlag = 1; //set command flag to indicate received coordinates
|
| sleighton |
10:cb1d91c06093
|
244
|
break;
|
| sleighton |
10:cb1d91c06093
|
245
|
}
|
| sleighton |
7:c3acafdb70c0
|
246
|
default:
|
| sleighton |
7:c3acafdb70c0
|
247
|
printf("Received data command not recognised: %c",command);
|
| sleighton |
7:c3acafdb70c0
|
248
|
}
|
| sleighton |
10:cb1d91c06093
|
249
|
}
|
| sleighton |
10:cb1d91c06093
|
250
|
|
| sleighton |
10:cb1d91c06093
|
251
|
int XBee_Robot::getCommandX(){
|
| sleighton |
10:cb1d91c06093
|
252
|
return commandX;
|
| sleighton |
10:cb1d91c06093
|
253
|
}
|
| sleighton |
10:cb1d91c06093
|
254
|
|
| sleighton |
10:cb1d91c06093
|
255
|
int XBee_Robot::getCommandY(){
|
| sleighton |
10:cb1d91c06093
|
256
|
return commandY;
|
| sleighton |
10:cb1d91c06093
|
257
|
}
|
| sleighton |
10:cb1d91c06093
|
258
|
|
| sleighton |
10:cb1d91c06093
|
259
|
int XBee_Robot::getCommandFlag(){
|
| sleighton |
10:cb1d91c06093
|
260
|
return commandFlag; //return value of command flag
|
| sleighton |
10:cb1d91c06093
|
261
|
}
|
| sleighton |
10:cb1d91c06093
|
262
|
|
| sleighton |
10:cb1d91c06093
|
263
|
void XBee_Robot::resetCommandFlag(){
|
| sleighton |
10:cb1d91c06093
|
264
|
commandFlag = 0; //reset command flag
|
| sleighton |
7:c3acafdb70c0
|
265
|
} |