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