root / trunk / code / projects / diagnostic_station / station / comm_robot.c @ 1295
History | View | Annotate | Download (9.11 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wireless.h> |
3 |
#include <string.h> |
4 |
|
5 |
#include "global.h" |
6 |
#include "comm_robot.h" |
7 |
#include "../common/comm_station_robot.h" |
8 |
|
9 |
|
10 |
#define received_state_waiting 0 |
11 |
#define received_state_pre_timeout 1 |
12 |
#define received_state_timeout 2 |
13 |
#define received_state_received 3 |
14 |
#define received_state_uninitialized 4 |
15 |
|
16 |
#define MAX_PACKET_LENGTH 32 |
17 |
|
18 |
uint8_t received_buffer[MAX_PACKET_LENGTH]; // Maximum length of a packet to be handled
|
19 |
uint8_t received_length; |
20 |
uint8_t received_state; |
21 |
|
22 |
|
23 |
|
24 |
// *********************
|
25 |
// ** Motor direction **
|
26 |
// *********************
|
27 |
|
28 |
/**
|
29 |
* All the functions in this file use these motor settings: a pair (direction/velocity) maps to a pair motor_direction/
|
30 |
* motor_velocity. motor_direction and motor_velocity are the values passed to the library functions and also sent
|
31 |
* to the robot. direction also includes an "off" direction. If the direction is off, the motor_velocity is always 0.\
|
32 |
**/
|
33 |
|
34 |
char *motor_direction_string (uint8_t direction)
|
35 |
{ |
36 |
if (direction==motor_direction_forward)
|
37 |
return "forward"; |
38 |
else if (direction==motor_direction_backward) |
39 |
return "backward"; |
40 |
else if (direction==motor_direction_off) |
41 |
return "off"; |
42 |
else
|
43 |
return "?"; |
44 |
} |
45 |
|
46 |
static uint8_t motor_direction (uint8_t direction, uint8_t velocity)
|
47 |
{ |
48 |
if (direction==motor_direction_backward)
|
49 |
return BACKWARD;
|
50 |
else
|
51 |
return FORWARD;
|
52 |
} |
53 |
|
54 |
static uint8_t motor_velocity (uint8_t direction, uint8_t velocity)
|
55 |
{ |
56 |
if (direction==motor_direction_off)
|
57 |
return 0; |
58 |
else
|
59 |
return velocity;
|
60 |
} |
61 |
|
62 |
|
63 |
// ***************************
|
64 |
// ** Common comm functions **
|
65 |
// ***************************
|
66 |
|
67 |
static void send_packet (char type, char *data, int len) |
68 |
{ |
69 |
// Right now we only wait until we receive *any* packet (with the appropriate group), so the functionality for
|
70 |
// sending command packets and request packets is the same.
|
71 |
// Note that this will horribly fail if other people are using the same channel/group.
|
72 |
|
73 |
// Keep sending the packet until we get a reply (retry on timeout)
|
74 |
do
|
75 |
{ |
76 |
received_state=received_state_waiting; |
77 |
|
78 |
// Send the request packet
|
79 |
wl_send_global_packet (station_robot_group, type, data, len, 0);
|
80 |
|
81 |
// When the reply is received, the receive handler will set received_state to recieved_state_received. Wait
|
82 |
// until this happens, periodically calling the wl_do function to handle incoming messages.
|
83 |
|
84 |
// Keep polling the wireless until the state is either "received" or "timeout" (as set by the callbacks)
|
85 |
while (received_state!=received_state_received && received_state!=received_state_timeout)
|
86 |
wl_do(); |
87 |
|
88 |
if (received_state==received_state_timeout)
|
89 |
usb_puts ("# Timeout, retrying" NL);
|
90 |
} |
91 |
while (received_state==received_state_timeout);
|
92 |
} |
93 |
|
94 |
// A packet that is followed by a "done" packet from the robot.
|
95 |
static void send_command_packet (char type, char *data, int len) |
96 |
{ |
97 |
send_packet (type, data, len); |
98 |
} |
99 |
|
100 |
// A packet that is followed by a data packet from the robot
|
101 |
static void send_request_packet(char type, char *data, int len) |
102 |
{ |
103 |
send_packet (type, data, len); |
104 |
} |
105 |
|
106 |
|
107 |
// *************************
|
108 |
// ** Individual commands **
|
109 |
// *************************
|
110 |
|
111 |
void robot_set_orbs (uint8_t red1, uint8_t green1, uint8_t blue1, uint8_t red2, uint8_t green2, uint8_t blue2)
|
112 |
{ |
113 |
char data[6]; |
114 |
|
115 |
data[0]=red1;
|
116 |
data[1]=green1;
|
117 |
data[2]=blue1;
|
118 |
|
119 |
data[3]=red2;
|
120 |
data[4]=green2;
|
121 |
data[5]=blue2;
|
122 |
|
123 |
send_command_packet (station_robot_set_orbs, data, 6);
|
124 |
} |
125 |
|
126 |
/** Direction is motor_direction_off/forward/backward, not FORWARD/BACKWARD */
|
127 |
void robot_set_motors (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2)
|
128 |
{ |
129 |
char data[4]; |
130 |
|
131 |
data[0]=motor_direction (direction1, speed1);
|
132 |
data[1]=motor_velocity (direction1, speed1);
|
133 |
data[2]=motor_direction (direction2, speed2);
|
134 |
data[3]=motor_velocity (direction2, speed2);
|
135 |
|
136 |
send_command_packet (station_robot_set_motors, data, 4);
|
137 |
} |
138 |
|
139 |
/** Direction is direction_off/forward/backward, not FORWARD/BACKWARD */
|
140 |
void robot_set_motors_time (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2, uint16_t time_ms)
|
141 |
{ |
142 |
char data[6]; |
143 |
|
144 |
data[0]=motor_direction (direction1, speed1);
|
145 |
data[1]=motor_velocity (direction1, speed1);
|
146 |
data[2]=motor_direction (direction2, speed1);
|
147 |
data[3]=motor_velocity (direction2, speed2);
|
148 |
data[4]=WORD_BYTE_0(time_ms);
|
149 |
data[5]=WORD_BYTE_1(time_ms);
|
150 |
|
151 |
send_command_packet (station_robot_set_motors_time, data, 6);
|
152 |
} |
153 |
|
154 |
void robot_set_motors_off (void) |
155 |
{ |
156 |
send_command_packet (station_robot_set_motors_off, NULL, 0); |
157 |
} |
158 |
|
159 |
void robot_set_bom (uint16_t bitmask)
|
160 |
{ |
161 |
char data[2]; |
162 |
|
163 |
data[0]=WORD_BYTE_0(bitmask);
|
164 |
data[1]=WORD_BYTE_1(bitmask);
|
165 |
|
166 |
send_command_packet (station_robot_set_bom, data, 2);
|
167 |
} |
168 |
|
169 |
void robot_reset_encoders (void) |
170 |
{ |
171 |
send_command_packet (station_robot_reset_encoders, NULL, 0); |
172 |
} |
173 |
|
174 |
|
175 |
// *************************
|
176 |
// ** Individual requests **
|
177 |
// *************************
|
178 |
|
179 |
bool robot_read_encoders (int16_t *left, int16_t *right)
|
180 |
{ |
181 |
send_request_packet (station_robot_read_encoders, NULL, 0); |
182 |
|
183 |
// Now the received data is in received_buffer/received_length
|
184 |
if (received_length>=4) |
185 |
{ |
186 |
*left =WORD(received_buffer[0], received_buffer[1]); |
187 |
*right=WORD(received_buffer[2], received_buffer[3]); |
188 |
return true; |
189 |
} |
190 |
else
|
191 |
{ |
192 |
return false; |
193 |
} |
194 |
} |
195 |
|
196 |
bool robot_read_bom (uint8_t num, uint16_t *value)
|
197 |
{ |
198 |
send_request_packet(station_robot_read_bom, (char *)&num, 1); |
199 |
|
200 |
// Now the received data is in received_buffer/received_length
|
201 |
if(received_length >= 2) |
202 |
{ |
203 |
*value = WORD(received_buffer[0], received_buffer[1]); |
204 |
return true; |
205 |
} |
206 |
else
|
207 |
{ |
208 |
return false; |
209 |
} |
210 |
|
211 |
} |
212 |
|
213 |
bool robot_read_rangefinder (uint8_t num, uint16_t *value)
|
214 |
{ |
215 |
send_request_packet(station_robot_read_rangefinder, (char *)&num, 1); |
216 |
|
217 |
// Now the received data is in received_buffer/received_length
|
218 |
if(received_length >= 2) |
219 |
{ |
220 |
*value = WORD(received_buffer[0], received_buffer[1]); |
221 |
return true; |
222 |
} |
223 |
else
|
224 |
{ |
225 |
return false; |
226 |
} |
227 |
} |
228 |
|
229 |
bool robot_read_bom_all (int16_t *value)
|
230 |
{ |
231 |
send_request_packet(station_robot_read_bom_all, NULL, 0); |
232 |
|
233 |
// Now the received data is in received_buffer/received_length
|
234 |
if(received_length >= 32) |
235 |
{ |
236 |
value[0] = WORD(received_buffer[0], received_buffer[1]); |
237 |
value[1] = WORD(received_buffer[2], received_buffer[3]); |
238 |
value[2] = WORD(received_buffer[4], received_buffer[5]); |
239 |
value[3] = WORD(received_buffer[6], received_buffer[7]); |
240 |
value[4] = WORD(received_buffer[8], received_buffer[9]); |
241 |
value[5] = WORD(received_buffer[10], received_buffer[11]); |
242 |
value[6] = WORD(received_buffer[12], received_buffer[13]); |
243 |
value[7] = WORD(received_buffer[14], received_buffer[15]); |
244 |
value[8] = WORD(received_buffer[16], received_buffer[17]); |
245 |
value[9] = WORD(received_buffer[18], received_buffer[19]); |
246 |
value[10] = WORD(received_buffer[20], received_buffer[21]); |
247 |
value[11] = WORD(received_buffer[22], received_buffer[23]); |
248 |
value[12] = WORD(received_buffer[24], received_buffer[25]); |
249 |
value[13] = WORD(received_buffer[26], received_buffer[27]); |
250 |
value[14] = WORD(received_buffer[28], received_buffer[29]); |
251 |
value[15] = WORD(received_buffer[30], received_buffer[31]); |
252 |
return true; |
253 |
} |
254 |
else
|
255 |
{ |
256 |
return false; |
257 |
} |
258 |
} |
259 |
|
260 |
|
261 |
// ***********************
|
262 |
// ** Receive callbacks **
|
263 |
// ***********************
|
264 |
|
265 |
static void robot_station_receive (char type, int source, unsigned char* packet, int length) |
266 |
{ |
267 |
// Dump the raw packet
|
268 |
//usb_puts ("# [");
|
269 |
//for (uint8_t i=0; i<length; ++i)
|
270 |
//{
|
271 |
// if (i!=0) usb_putc (' ');
|
272 |
// usb_puti (packet[i]);
|
273 |
//}
|
274 |
//usb_putc (']');
|
275 |
|
276 |
// We received some data from the robot. It is not handled immediately as some function is waiting for it.
|
277 |
|
278 |
// Note the fact that we received some data
|
279 |
received_state=received_state_received; |
280 |
|
281 |
// The packet buffer might be overwritten after this function returns, so we have to make a copy of the data.
|
282 |
|
283 |
// First, determine the number of bytes to copy. It is the minimum of the received length and the size of the data
|
284 |
// buffer the data is copied to. TODO: signal an error if this happens.
|
285 |
received_length=length; |
286 |
if (sizeof (received_buffer)<received_length) received_length=sizeof (received_buffer); |
287 |
|
288 |
// Now copy the data
|
289 |
memcpy (received_buffer, packet, received_length); |
290 |
} |
291 |
|
292 |
static void robot_station_timeout (void) |
293 |
{ |
294 |
if (received_state==received_state_waiting ) { received_state=received_state_pre_timeout; }
|
295 |
else if (received_state==received_state_pre_timeout) { received_state=received_state_timeout ; } |
296 |
} |
297 |
|
298 |
// ********************
|
299 |
// ** Initialization **
|
300 |
// ********************
|
301 |
|
302 |
// Must not be a local variable because the library doesn't make a copy of it.
|
303 |
PacketGroupHandler receive_handler={robot_station_group, robot_station_timeout, NULL, &robot_station_receive, NULL}; |
304 |
|
305 |
void comm_robot_init ()
|
306 |
{ |
307 |
received_state=received_state_uninitialized; |
308 |
received_length=0;
|
309 |
|
310 |
usb_puts("# Initializing robot communication" NL);
|
311 |
xbee_init (); |
312 |
wl_init(); |
313 |
usb_puts("# Done" NL);
|
314 |
|
315 |
//PacketGroupHandler xxx_receive_handler = {robot_station_group, NULL, NULL, &robot_station_receive, NULL};
|
316 |
wl_register_packet_group(&receive_handler); |
317 |
} |