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