root / branches / autonomous_recharging / code / projects / autonomous_recharging / charging_station / wl_charging_station.c @ 363
History | View | Annotate | Download (10.6 KB)
1 | 98 | bcoltin | #include "wl_charging_station.h" |
---|---|---|---|
2 | |||
3 | #include <wireless.h> |
||
4 | #include <wl_defs.h> |
||
5 | 120 | bcoltin | #include <serial.h> |
6 | 98 | bcoltin | |
7 | #include "charging_defs.h" |
||
8 | #include "charging.h" |
||
9 | |||
10 | 120 | bcoltin | #define NULL 0 |
11 | |||
12 | 98 | bcoltin | //Possible states for the robot
|
13 | /** The robot is not attempting to recharging. **/
|
||
14 | #define NOT_RECHARGING 0 |
||
15 | /** Waiting for an available charging station. **/
|
||
16 | #define POLLING 1 |
||
17 | /** Requesting to dock with a charging station. **/
|
||
18 | #define REQUESTING 2 |
||
19 | /** Traveling to a charging station. **/
|
||
20 | #define SEEKING 3 |
||
21 | /** Docked with a charging station. **/
|
||
22 | #define DOCKED 4 |
||
23 | /** Leaving a charging station. **/
|
||
24 | #define DEPARTING 5 |
||
25 | |||
26 | // Function prototypes
|
||
27 | void wl_charging_timeout_handler(void); |
||
28 | void wl_charging_response_handler(int frame, int received); |
||
29 | void wl_charging_handle_receive(char type, int source, unsigned char* packet, |
||
30 | int length);
|
||
31 | void wl_charging_cleanup(void); |
||
32 | |||
33 | // messgae handlers
|
||
34 | void wl_charging_poll(int source); |
||
35 | void wl_charging_request(int source); |
||
36 | void wl_charging_verify(int source); |
||
37 | void wl_charging_cancel(int source); |
||
38 | void wl_charging_seeking(int source); |
||
39 | void wl_charging_docked(int source); |
||
40 | |||
41 | // sending packets
|
||
42 | void wl_charging_send_station_available(int dest); |
||
43 | void wl_charging_send_request_accept(int dest); |
||
44 | void wl_charging_send_verify(int dest); |
||
45 | void wl_charging_send_cancel(int dest); |
||
46 | void wl_charging_send_seeking(int dest); |
||
47 | void wl_charging_send_docked(int dest); |
||
48 | 363 | chihsiuh | void wl_charging_send_battery(int dest); |
49 | 98 | bcoltin | |
50 | // the handler
|
||
51 | PacketGroupHandler wl_charging_handler = |
||
52 | {WL_RECHARGE_GROUP, wl_charging_timeout_handler, |
||
53 | wl_charging_response_handler, wl_charging_handle_receive, |
||
54 | wl_charging_cleanup}; |
||
55 | |||
56 | |||
57 | //frames
|
||
58 | #define STATION_AVAILABLE_FRAME 1 |
||
59 | #define REQUEST_ACCEPT_FRAME 2 |
||
60 | #define VERIFY_FRAME 3 |
||
61 | #define CANCEL_FRAME 4 |
||
62 | #define SEEKING_FRAME 5 |
||
63 | #define DOCKED_FRAME 6 |
||
64 | 363 | chihsiuh | #define BATTERY_FRAME 7 |
65 | 98 | bcoltin | |
66 | /**
|
||
67 | * Register this packet group with the wireless library.
|
||
68 | * This function must be called before any other wl_charging
|
||
69 | * function. Also, wl_token_ring_register must be called
|
||
70 | * before this function, along with wl_token_ring_join.
|
||
71 | *
|
||
72 | * @see wl_charging_unregister
|
||
73 | **/
|
||
74 | void wl_station_register(void) |
||
75 | { |
||
76 | wl_register_packet_group(&wl_charging_handler); |
||
77 | } |
||
78 | |||
79 | /**
|
||
80 | * Called to unregister this packet group with the wireless
|
||
81 | * library.
|
||
82 | *
|
||
83 | * @see wl_charging_register
|
||
84 | **/
|
||
85 | void wl_station_unregister(void) |
||
86 | { |
||
87 | wl_unregister_packet_group(&wl_charging_handler); |
||
88 | } |
||
89 | |||
90 | /**
|
||
91 | * Sends a packet alerting a specific robot that the station
|
||
92 | * is available.
|
||
93 | *
|
||
94 | * @param dest the robot to tell that the station is available
|
||
95 | **/
|
||
96 | void wl_charging_send_station_available(int dest) |
||
97 | { |
||
98 | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, |
||
99 | WL_RECHARGE_STATION_AVAILABLE, NULL, 0, dest, |
||
100 | STATION_AVAILABLE_FRAME); |
||
101 | } |
||
102 | |||
103 | /**
|
||
104 | * Sends a packet accepting the request of a specific robot.
|
||
105 | *
|
||
106 | * @param dest the robot to alert of its acceptance
|
||
107 | **/
|
||
108 | void wl_charging_send_request_accept(int dest) |
||
109 | { |
||
110 | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, |
||
111 | WL_RECHARGE_REQUEST_ACCEPT, NULL, 0, dest, |
||
112 | REQUEST_ACCEPT_FRAME); |
||
113 | } |
||
114 | |||
115 | /**
|
||
116 | * Sends a request to verify what the robot
|
||
117 | * believes it is doing.
|
||
118 | *
|
||
119 | * @param dest the robot to check
|
||
120 | **/
|
||
121 | void wl_charging_send_verify(int dest) |
||
122 | { |
||
123 | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_VERIFY, |
||
124 | NULL, 0, dest, VERIFY_FRAME); |
||
125 | } |
||
126 | |||
127 | /**
|
||
128 | * Sends a packet to the robot that we are
|
||
129 | * no longer allowing it to charge.
|
||
130 | *
|
||
131 | * @param dest the robot to tell that it cannot charge
|
||
132 | **/
|
||
133 | void wl_charging_send_cancel(int dest) |
||
134 | { |
||
135 | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_CANCEL, |
||
136 | NULL, 0, dest, CANCEL_FRAME); |
||
137 | } |
||
138 | |||
139 | /**
|
||
140 | * Sends a packet to the robot that it may
|
||
141 | * attempt to dock with us.
|
||
142 | *
|
||
143 | * @param dest the robot attempting to dock
|
||
144 | **/
|
||
145 | void wl_charging_send_seeking(int dest) |
||
146 | { |
||
147 | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_SEEKING, |
||
148 | NULL, 0, dest, SEEKING_FRAME); |
||
149 | } |
||
150 | |||
151 | /**
|
||
152 | * Sends a packet to the robot that
|
||
153 | * we believe it is docked with us.
|
||
154 | *
|
||
155 | * @param dest the robot that has docked with us
|
||
156 | **/
|
||
157 | void wl_charging_send_docked(int dest) |
||
158 | { |
||
159 | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_DOCKED, |
||
160 | NULL, 0, dest, DOCKED_FRAME); |
||
161 | } |
||
162 | |||
163 | /**
|
||
164 | 363 | chihsiuh | * Sends a packet to request for the robot's battery reading
|
165 | *
|
||
166 | * @param dest the robot to request
|
||
167 | **/
|
||
168 | void wl_charging_send_battery(int dest) |
||
169 | { |
||
170 | wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_BATTERY, NULL, 0, dest, BATTERY_FRAME); |
||
171 | } |
||
172 | |||
173 | /**
|
||
174 | 98 | bcoltin | * Called when this packet is unregistered with the wireless library.
|
175 | **/
|
||
176 | void wl_charging_cleanup(void) |
||
177 | { |
||
178 | |||
179 | } |
||
180 | |||
181 | /**
|
||
182 | * Called when the timer goes off in the wireless library.
|
||
183 | **/
|
||
184 | void wl_charging_timeout_handler(void) |
||
185 | { |
||
186 | charging_robot_iterator_init(); |
||
187 | |||
188 | while (charging_robot_iterator_has_next())
|
||
189 | { |
||
190 | int robot = charging_robot_iterator_next();
|
||
191 | |||
192 | int c = charging_get_verify_count(robot);
|
||
193 | if (c <= 0) |
||
194 | { |
||
195 | 120 | bcoltin | STATION_DEBUG_PRINT("Robot ");
|
196 | STATION_DEBUG_PUTI(robot); |
||
197 | STATION_DEBUG_PRINT(" has timed out.");
|
||
198 | |||
199 | 98 | bcoltin | charging_cancel(robot); |
200 | continue;
|
||
201 | } |
||
202 | |||
203 | if (c % VERIFY_FREQUENCY == 0 && c != VERIFY_DELAY) |
||
204 | 120 | bcoltin | { |
205 | STATION_DEBUG_PRINT("Verification sent to robot ");
|
||
206 | STATION_DEBUG_PUTI(robot); |
||
207 | STATION_DEBUG_PRINT(".\n");
|
||
208 | 98 | bcoltin | wl_charging_send_verify(robot); |
209 | 120 | bcoltin | } |
210 | 98 | bcoltin | |
211 | charging_set_verify_count(robot, c - 1);
|
||
212 | } |
||
213 | } |
||
214 | |||
215 | /**
|
||
216 | * Called when a response is received regarding a packet that was sent.
|
||
217 | *
|
||
218 | * @param frame the frame number for the packet
|
||
219 | * @param received true if the packet was received, false otherwise
|
||
220 | **/
|
||
221 | void wl_charging_response_handler(int frame, int received) |
||
222 | { |
||
223 | if (received)
|
||
224 | return;
|
||
225 | if (!received)
|
||
226 | { |
||
227 | STATION_DEBUG_PRINT("Sent packet not recieved.\n");
|
||
228 | switch (frame)
|
||
229 | { |
||
230 | case STATION_AVAILABLE_FRAME:
|
||
231 | break;
|
||
232 | case REQUEST_ACCEPT_FRAME:
|
||
233 | break;
|
||
234 | case VERIFY_FRAME:
|
||
235 | break;
|
||
236 | case CANCEL_FRAME:
|
||
237 | case SEEKING_FRAME:
|
||
238 | case DOCKED_FRAME:
|
||
239 | break;
|
||
240 | } |
||
241 | } |
||
242 | } |
||
243 | |||
244 | /**
|
||
245 | * Handles receiving an error packet.
|
||
246 | *
|
||
247 | * @param type the packet type
|
||
248 | * @param source the 16-bit address of the packet's sender
|
||
249 | * @param packet the packet data
|
||
250 | * @param length the length in bytes of the packet
|
||
251 | **/
|
||
252 | void wl_charging_handle_receive(char type, int source, unsigned char* packet, |
||
253 | int length)
|
||
254 | { |
||
255 | switch (type)
|
||
256 | { |
||
257 | case WL_RECHARGE_POLL_STATIONS:
|
||
258 | wl_charging_poll(source); |
||
259 | break;
|
||
260 | case WL_RECHARGE_STATION_AVAILABLE:
|
||
261 | STATION_DEBUG_PRINT("Stations should not receive ");
|
||
262 | STATION_DEBUG_PRINT("station available packets.\n");
|
||
263 | break;
|
||
264 | case WL_RECHARGE_REQUEST:
|
||
265 | wl_charging_request(source); |
||
266 | break;
|
||
267 | case WL_RECHARGE_REQUEST_ACCEPT:
|
||
268 | STATION_DEBUG_PRINT("Stations should not receive ");
|
||
269 | STATION_DEBUG_PRINT("request accept packets.\n");
|
||
270 | break;
|
||
271 | case WL_RECHARGE_VERIFY:
|
||
272 | wl_charging_verify(source); |
||
273 | break;
|
||
274 | case WL_RECHARGE_CANCEL:
|
||
275 | wl_charging_cancel(source); |
||
276 | break;
|
||
277 | case WL_RECHARGE_SEEKING:
|
||
278 | wl_charging_seeking(source); |
||
279 | break;
|
||
280 | case WL_RECHARGE_DOCKED:
|
||
281 | wl_charging_docked(source); |
||
282 | break;
|
||
283 | 363 | chihsiuh | case WL_RECHARGE_BATTERY:
|
284 | wl_charging_battery(source,packet,length); |
||
285 | 98 | bcoltin | default:
|
286 | STATION_DEBUG_PRINT("Error packet of unknown type received.\n");
|
||
287 | break;
|
||
288 | } |
||
289 | } |
||
290 | |||
291 | /**
|
||
292 | * Called when we receive a packet alerting us that
|
||
293 | * a robot is looking for stations.
|
||
294 | *
|
||
295 | * @param source the robot requesting a station
|
||
296 | **/
|
||
297 | void wl_charging_poll(int source) |
||
298 | { |
||
299 | if (charging_is_space_available() && !charging_is_robot_seeking())
|
||
300 | wl_charging_send_station_available(source); |
||
301 | 363 | chihsiuh | wl_charging_send_battery(source); |
302 | 98 | bcoltin | } |
303 | |||
304 | /**
|
||
305 | * Called when we receive a request to charge.
|
||
306 | *
|
||
307 | * @param source the robot requesting to charge
|
||
308 | **/
|
||
309 | void wl_charging_request(int source) |
||
310 | { |
||
311 | if (charging_is_space_available() && !charging_is_robot_seeking())
|
||
312 | { |
||
313 | charging_begin_seeking(source); |
||
314 | wl_charging_send_request_accept(source); |
||
315 | } |
||
316 | else
|
||
317 | wl_charging_send_cancel(source); |
||
318 | } |
||
319 | |||
320 | /**
|
||
321 | * Called when we receive a request to verify our status
|
||
322 | * in the docking procedure.
|
||
323 | *
|
||
324 | * @param source the robot verifying our status
|
||
325 | **/
|
||
326 | void wl_charging_verify(int source) |
||
327 | { |
||
328 | int state = charging_get_robot_state(source);
|
||
329 | |||
330 | 120 | bcoltin | STATION_DEBUG_PRINT("Received verify request from robot ");
|
331 | STATION_DEBUG_PUTI(source); |
||
332 | STATION_DEBUG_PRINT(".\n");
|
||
333 | |||
334 | 98 | bcoltin | switch (state)
|
335 | { |
||
336 | case STATE_SEEKING:
|
||
337 | wl_charging_send_seeking(source); |
||
338 | break;
|
||
339 | case STATE_DOCKED:
|
||
340 | wl_charging_send_docked(source); |
||
341 | break;
|
||
342 | case STATE_NOT_CHARGING:
|
||
343 | 120 | bcoltin | STATION_DEBUG_PRINT("Received verify from unknown robot ");
|
344 | STATION_DEBUG_PUTI(source); |
||
345 | STATION_DEBUG_PRINT(".\n");
|
||
346 | 98 | bcoltin | wl_charging_send_cancel(source); |
347 | break;
|
||
348 | default:
|
||
349 | STATION_DEBUG_PRINT("Unknown robot state.\n");
|
||
350 | break;
|
||
351 | } |
||
352 | } |
||
353 | |||
354 | /**
|
||
355 | * Called when we receive a packet cancelling our
|
||
356 | * docking procedure.
|
||
357 | *
|
||
358 | * @param source the robot cancelling the procedure
|
||
359 | **/
|
||
360 | void wl_charging_cancel(int source) |
||
361 | { |
||
362 | int state = charging_get_robot_state(source);
|
||
363 | |||
364 | 120 | bcoltin | STATION_DEBUG_PRINT("Robot ");
|
365 | STATION_DEBUG_PUTI(source); |
||
366 | STATION_DEBUG_PRINT(" has cancelled charging.\n");
|
||
367 | |||
368 | 98 | bcoltin | if (state == STATE_NOT_CHARGING)
|
369 | { |
||
370 | STATION_DEBUG_PRINT("Received cancel from unknown robot.\n");
|
||
371 | return;
|
||
372 | } |
||
373 | |||
374 | charging_set_verify_count(source, VERIFY_DELAY); |
||
375 | |||
376 | charging_cancel(source); |
||
377 | } |
||
378 | |||
379 | /**
|
||
380 | * Called when we receive a packet stating that the
|
||
381 | * robot is seeking us.
|
||
382 | *
|
||
383 | * @param source the robot seeking us
|
||
384 | **/
|
||
385 | void wl_charging_seeking(int source) |
||
386 | { |
||
387 | int state = charging_get_robot_state(source);
|
||
388 | |||
389 | 120 | bcoltin | STATION_DEBUG_PRINT("Robot ");
|
390 | STATION_DEBUG_PUTI(source); |
||
391 | STATION_DEBUG_PRINT(" has confirmed it is seeking.\n");
|
||
392 | |||
393 | 98 | bcoltin | if (state == STATE_NOT_CHARGING)
|
394 | { |
||
395 | STATION_DEBUG_PRINT("Received seeking from unknown robot.\n");
|
||
396 | wl_charging_send_cancel(source); |
||
397 | return;
|
||
398 | } |
||
399 | |||
400 | charging_set_verify_count(source, VERIFY_DELAY); |
||
401 | |||
402 | if (state == STATE_DOCKED)
|
||
403 | { |
||
404 | STATION_DEBUG_PRINT("Should not have revert from docked to seeking.\n");
|
||
405 | if (charging_is_robot_seeking())
|
||
406 | { |
||
407 | charging_cancel(source); |
||
408 | wl_charging_send_cancel(source); |
||
409 | } |
||
410 | else
|
||
411 | charging_begin_seeking(source); |
||
412 | } |
||
413 | } |
||
414 | |||
415 | /**
|
||
416 | * Called when we receive a packet stating that
|
||
417 | * a robot is docked with us.
|
||
418 | *
|
||
419 | * @param source the robot that is docked with us
|
||
420 | **/
|
||
421 | void wl_charging_docked(int source) |
||
422 | { |
||
423 | int state = charging_get_robot_state(source);
|
||
424 | 120 | bcoltin | |
425 | STATION_DEBUG_PRINT("Robot ");
|
||
426 | STATION_DEBUG_PUTI(source); |
||
427 | STATION_DEBUG_PRINT(" has confirmed it is docked.\n");
|
||
428 | |||
429 | 98 | bcoltin | if (state == STATE_NOT_CHARGING)
|
430 | { |
||
431 | STATION_DEBUG_PRINT("Received docked from unknown robot.\n");
|
||
432 | return;
|
||
433 | } |
||
434 | |||
435 | charging_set_verify_count(source, VERIFY_DELAY); |
||
436 | |||
437 | //seeking has ended
|
||
438 | if (state == STATE_SEEKING)
|
||
439 | charging_dock(source); |
||
440 | } |
||
441 | |||
442 | 363 | chihsiuh | /**
|
443 | * Called when we recieve a packet with a battery reading
|
||
444 | *
|
||
445 | * @param source the robot that is sending the battery reading
|
||
446 | **/
|
||
447 | void wl_charging_battery(int source, unsigned char* packet, int length) |
||
448 | { |
||
449 | if (charging_get_robot_state(source)==STATE_NOT_CHARGING)
|
||
450 | check_battery(source, (int)packet[0]); |
||
451 | else
|
||
452 | update_battery(source, (int)packet[0]); |
||
453 | } |