root / trunk / code / projects / diagnostic_station / station / comm_robot.c @ 1300
History | View | Annotate | Download (9.37 KB)
1 | 1159 | deffi | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wireless.h> |
||
3 | 1218 | deffi | #include <string.h> |
4 | 1159 | deffi | |
5 | 1218 | deffi | #include "global.h" |
6 | 1182 | deffi | #include "comm_robot.h" |
7 | 1159 | deffi | #include "../common/comm_station_robot.h" |
8 | |||
9 | 1192 | emullini | |
10 | 1226 | deffi | #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 | 1282 | deffi | #define MAX_PACKET_LENGTH 32 |
17 | |||
18 | uint8_t received_buffer[MAX_PACKET_LENGTH]; // Maximum length of a packet to be handled
|
||
19 | 1218 | deffi | uint8_t received_length; |
20 | 1226 | deffi | uint8_t received_state; |
21 | 1218 | deffi | |
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 | 1205 | deffi | 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 | 1218 | deffi | static uint8_t motor_direction (uint8_t direction, uint8_t velocity)
|
47 | 1204 | deffi | { |
48 | 1205 | deffi | if (direction==motor_direction_backward)
|
49 | 1204 | deffi | return BACKWARD;
|
50 | else
|
||
51 | return FORWARD;
|
||
52 | } |
||
53 | |||
54 | static uint8_t motor_velocity (uint8_t direction, uint8_t velocity)
|
||
55 | { |
||
56 | 1205 | deffi | if (direction==motor_direction_off)
|
57 | 1204 | deffi | return 0; |
58 | else
|
||
59 | return velocity;
|
||
60 | } |
||
61 | |||
62 | 1218 | deffi | |
63 | // ***************************
|
||
64 | // ** Common comm functions **
|
||
65 | // ***************************
|
||
66 | |||
67 | 1226 | deffi | 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 | 1164 | deffi | // 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 | 1159 | deffi | { |
97 | 1226 | deffi | send_packet (type, data, len); |
98 | 1159 | deffi | } |
99 | |||
100 | 1164 | deffi | // A packet that is followed by a data packet from the robot
|
101 | 1192 | emullini | static void send_request_packet(char type, char *data, int len) |
102 | { |
||
103 | 1226 | deffi | send_packet (type, data, len); |
104 | 1192 | emullini | } |
105 | 1164 | deffi | |
106 | |||
107 | 1218 | deffi | // *************************
|
108 | // ** Individual commands **
|
||
109 | // *************************
|
||
110 | 1164 | deffi | |
111 | 1159 | deffi | 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 | 1164 | deffi | send_command_packet (station_robot_set_orbs, data, 6);
|
124 | 1159 | deffi | } |
125 | |||
126 | 1205 | deffi | /** Direction is motor_direction_off/forward/backward, not FORWARD/BACKWARD */
|
127 | 1159 | deffi | void robot_set_motors (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2)
|
128 | { |
||
129 | char data[4]; |
||
130 | |||
131 | 1218 | deffi | 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 | 1159 | deffi | |
136 | 1164 | deffi | send_command_packet (station_robot_set_motors, data, 4);
|
137 | 1159 | deffi | } |
138 | |||
139 | 1205 | deffi | /** Direction is direction_off/forward/backward, not FORWARD/BACKWARD */
|
140 | 1164 | deffi | 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 | 1218 | deffi | 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 | 1164 | deffi | data[4]=WORD_BYTE_0(time_ms);
|
149 | data[5]=WORD_BYTE_1(time_ms);
|
||
150 | |||
151 | 1226 | deffi | send_command_packet (station_robot_set_motors_time, data, 6);
|
152 | 1164 | deffi | } |
153 | |||
154 | 1181 | deffi | void robot_set_motors_off (void) |
155 | 1164 | deffi | { |
156 | send_command_packet (station_robot_set_motors_off, NULL, 0); |
||
157 | } |
||
158 | |||
159 | 1159 | deffi | 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 | 1164 | deffi | send_command_packet (station_robot_set_bom, data, 2);
|
167 | 1159 | deffi | } |
168 | 1170 | emullini | |
169 | 1219 | deffi | void robot_reset_encoders (void) |
170 | { |
||
171 | send_command_packet (station_robot_reset_encoders, NULL, 0); |
||
172 | } |
||
173 | 1218 | deffi | |
174 | 1219 | deffi | |
175 | 1218 | deffi | // *************************
|
176 | // ** Individual requests **
|
||
177 | // *************************
|
||
178 | |||
179 | bool robot_read_encoders (int16_t *left, int16_t *right)
|
||
180 | 1192 | emullini | { |
181 | 1198 | deffi | send_request_packet (station_robot_read_encoders, NULL, 0); |
182 | 1218 | deffi | |
183 | 1226 | deffi | // Now the received data is in received_buffer/received_length
|
184 | 1218 | deffi | 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 | 1192 | emullini | } |
195 | |||
196 | 1245 | deffi | bool robot_read_bom (uint8_t num, uint16_t *value)
|
197 | 1212 | deffi | { |
198 | 1261 | deffi | send_request_packet(station_robot_read_bom, (char *)&num, 1); |
199 | 1260 | emullini | |
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 | 1212 | deffi | } |
212 | |||
213 | 1245 | deffi | bool robot_read_rangefinder (uint8_t num, uint16_t *value)
|
214 | 1213 | deffi | { |
215 | 1261 | deffi | send_request_packet(station_robot_read_rangefinder, (char *)&num, 1); |
216 | 1249 | emullini | |
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 | 1213 | deffi | } |
228 | 1212 | deffi | |
229 | 1282 | deffi | bool robot_read_bom_all (int16_t *value)
|
230 | 1273 | emullini | { |
231 | 1279 | deffi | send_request_packet(station_robot_read_bom_all, NULL, 0); |
232 | 1273 | emullini | |
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 | 1198 | deffi | |
260 | 1273 | emullini | |
261 | 1218 | deffi | // ***********************
|
262 | // ** Receive callbacks **
|
||
263 | // ***********************
|
||
264 | |||
265 | static void robot_station_receive (char type, int source, unsigned char* packet, int length) |
||
266 | 1192 | emullini | { |
267 | 1218 | deffi | // Dump the raw packet
|
268 | 1220 | deffi | //usb_puts ("# [");
|
269 | 1218 | deffi | //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 | 1226 | deffi | received_state=received_state_received; |
280 | 1218 | deffi | |
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 | 1298 | deffi | // buffer the data is copied to. If the received length is greater than the buffer size, print an error message.
|
285 | // We could stop here (while (1);), but then any long packet would stop the program, even if it's unrelated to
|
||
286 | // what we're doing.
|
||
287 | 1218 | deffi | received_length=length; |
288 | 1298 | deffi | if (sizeof (received_buffer)<received_length) |
289 | { |
||
290 | received_length=sizeof (received_buffer);
|
||
291 | usb_puts ("# Error: received packet is too long for allocated buffer." NL);
|
||
292 | } |
||
293 | 1218 | deffi | |
294 | // Now copy the data
|
||
295 | memcpy (received_buffer, packet, received_length); |
||
296 | 1195 | emullini | } |
297 | |||
298 | 1226 | deffi | static void robot_station_timeout (void) |
299 | { |
||
300 | if (received_state==received_state_waiting ) { received_state=received_state_pre_timeout; }
|
||
301 | else if (received_state==received_state_pre_timeout) { received_state=received_state_timeout ; } |
||
302 | } |
||
303 | 1198 | deffi | |
304 | 1218 | deffi | // ********************
|
305 | // ** Initialization **
|
||
306 | // ********************
|
||
307 | |||
308 | // Must not be a local variable because the library doesn't make a copy of it.
|
||
309 | 1226 | deffi | PacketGroupHandler receive_handler={robot_station_group, robot_station_timeout, NULL, &robot_station_receive, NULL}; |
310 | 1218 | deffi | |
311 | 1198 | deffi | void comm_robot_init ()
|
312 | { |
||
313 | 1226 | deffi | received_state=received_state_uninitialized; |
314 | 1218 | deffi | received_length=0;
|
315 | |||
316 | usb_puts("# Initializing robot communication" NL);
|
||
317 | xbee_init (); |
||
318 | wl_init(); |
||
319 | usb_puts("# Done" NL);
|
||
320 | |||
321 | //PacketGroupHandler xxx_receive_handler = {robot_station_group, NULL, NULL, &robot_station_receive, NULL};
|
||
322 | 1198 | deffi | wl_register_packet_group(&receive_handler); |
323 | } |