Revision 98
Added new charging station code to charging station branch, it now compiles.
branches/autonomous_recharging/code/projects/autonomous_recharging/charging_station/wl_charging_station.c | ||
---|---|---|
1 |
#include "wl_charging_station.h" |
|
2 |
|
|
3 |
#include <wireless.h> |
|
4 |
#include <wl_defs.h> |
|
5 |
#include <stdio.h> |
|
6 |
#include <string.h> |
|
7 |
|
|
8 |
#include "charging_defs.h" |
|
9 |
#include "charging.h" |
|
10 |
|
|
11 |
//Possible states for the robot |
|
12 |
/** The robot is not attempting to recharging. **/ |
|
13 |
#define NOT_RECHARGING 0 |
|
14 |
/** Waiting for an available charging station. **/ |
|
15 |
#define POLLING 1 |
|
16 |
/** Requesting to dock with a charging station. **/ |
|
17 |
#define REQUESTING 2 |
|
18 |
/** Traveling to a charging station. **/ |
|
19 |
#define SEEKING 3 |
|
20 |
/** Docked with a charging station. **/ |
|
21 |
#define DOCKED 4 |
|
22 |
/** Leaving a charging station. **/ |
|
23 |
#define DEPARTING 5 |
|
24 |
|
|
25 |
// Function prototypes |
|
26 |
void wl_charging_timeout_handler(void); |
|
27 |
void wl_charging_response_handler(int frame, int received); |
|
28 |
void wl_charging_handle_receive(char type, int source, unsigned char* packet, |
|
29 |
int length); |
|
30 |
void wl_charging_cleanup(void); |
|
31 |
|
|
32 |
// messgae handlers |
|
33 |
void wl_charging_poll(int source); |
|
34 |
void wl_charging_request(int source); |
|
35 |
void wl_charging_verify(int source); |
|
36 |
void wl_charging_cancel(int source); |
|
37 |
void wl_charging_seeking(int source); |
|
38 |
void wl_charging_docked(int source); |
|
39 |
|
|
40 |
// sending packets |
|
41 |
void wl_charging_send_station_available(int dest); |
|
42 |
void wl_charging_send_request_accept(int dest); |
|
43 |
void wl_charging_send_verify(int dest); |
|
44 |
void wl_charging_send_cancel(int dest); |
|
45 |
void wl_charging_send_seeking(int dest); |
|
46 |
void wl_charging_send_docked(int dest); |
|
47 |
|
|
48 |
// the handler |
|
49 |
PacketGroupHandler wl_charging_handler = |
|
50 |
{WL_RECHARGE_GROUP, wl_charging_timeout_handler, |
|
51 |
wl_charging_response_handler, wl_charging_handle_receive, |
|
52 |
wl_charging_cleanup}; |
|
53 |
|
|
54 |
//timing |
|
55 |
//timer ticks between polls |
|
56 |
#define VERIFY_DELAY 8 |
|
57 |
#define VERIFY_FREQUENCY 4 |
|
58 |
|
|
59 |
//frames |
|
60 |
#define STATION_AVAILABLE_FRAME 1 |
|
61 |
#define REQUEST_ACCEPT_FRAME 2 |
|
62 |
#define VERIFY_FRAME 3 |
|
63 |
#define CANCEL_FRAME 4 |
|
64 |
#define SEEKING_FRAME 5 |
|
65 |
#define DOCKED_FRAME 6 |
|
66 |
|
|
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 |
* Called when this packet is unregistered with the wireless library. |
|
166 |
**/ |
|
167 |
void wl_charging_cleanup(void) |
|
168 |
{ |
|
169 |
|
|
170 |
} |
|
171 |
|
|
172 |
/** |
|
173 |
* Called when the timer goes off in the wireless library. |
|
174 |
**/ |
|
175 |
void wl_charging_timeout_handler(void) |
|
176 |
{ |
|
177 |
charging_robot_iterator_init(); |
|
178 |
|
|
179 |
while (charging_robot_iterator_has_next()) |
|
180 |
{ |
|
181 |
int robot = charging_robot_iterator_next(); |
|
182 |
|
|
183 |
int c = charging_get_verify_count(robot); |
|
184 |
if (c <= 0) |
|
185 |
{ |
|
186 |
charging_cancel(robot); |
|
187 |
continue; |
|
188 |
} |
|
189 |
|
|
190 |
if (c % VERIFY_FREQUENCY == 0 && c != VERIFY_DELAY) |
|
191 |
wl_charging_send_verify(robot); |
|
192 |
|
|
193 |
charging_set_verify_count(robot, c - 1); |
|
194 |
} |
|
195 |
} |
|
196 |
|
|
197 |
/** |
|
198 |
* Called when a response is received regarding a packet that was sent. |
|
199 |
* |
|
200 |
* @param frame the frame number for the packet |
|
201 |
* @param received true if the packet was received, false otherwise |
|
202 |
**/ |
|
203 |
void wl_charging_response_handler(int frame, int received) |
|
204 |
{ |
|
205 |
if (received) |
|
206 |
return; |
|
207 |
if (!received) |
|
208 |
{ |
|
209 |
STATION_DEBUG_PRINT("Sent packet not recieved.\n"); |
|
210 |
switch (frame) |
|
211 |
{ |
|
212 |
case STATION_AVAILABLE_FRAME: |
|
213 |
break; |
|
214 |
case REQUEST_ACCEPT_FRAME: |
|
215 |
break; |
|
216 |
case VERIFY_FRAME: |
|
217 |
break; |
|
218 |
case CANCEL_FRAME: |
|
219 |
case SEEKING_FRAME: |
|
220 |
case DOCKED_FRAME: |
|
221 |
break; |
|
222 |
} |
|
223 |
} |
|
224 |
} |
|
225 |
|
|
226 |
/** |
|
227 |
* Handles receiving an error packet. |
|
228 |
* |
|
229 |
* @param type the packet type |
|
230 |
* @param source the 16-bit address of the packet's sender |
|
231 |
* @param packet the packet data |
|
232 |
* @param length the length in bytes of the packet |
|
233 |
**/ |
|
234 |
void wl_charging_handle_receive(char type, int source, unsigned char* packet, |
|
235 |
int length) |
|
236 |
{ |
|
237 |
switch (type) |
|
238 |
{ |
|
239 |
case WL_RECHARGE_POLL_STATIONS: |
|
240 |
wl_charging_poll(source); |
|
241 |
break; |
|
242 |
case WL_RECHARGE_STATION_AVAILABLE: |
|
243 |
STATION_DEBUG_PRINT("Stations should not receive "); |
|
244 |
STATION_DEBUG_PRINT("station available packets.\n"); |
|
245 |
break; |
|
246 |
case WL_RECHARGE_REQUEST: |
|
247 |
wl_charging_request(source); |
|
248 |
break; |
|
249 |
case WL_RECHARGE_REQUEST_ACCEPT: |
|
250 |
STATION_DEBUG_PRINT("Stations should not receive "); |
|
251 |
STATION_DEBUG_PRINT("request accept packets.\n"); |
|
252 |
break; |
|
253 |
case WL_RECHARGE_VERIFY: |
|
254 |
wl_charging_verify(source); |
|
255 |
break; |
|
256 |
case WL_RECHARGE_CANCEL: |
|
257 |
wl_charging_cancel(source); |
|
258 |
break; |
|
259 |
case WL_RECHARGE_SEEKING: |
|
260 |
wl_charging_seeking(source); |
|
261 |
break; |
|
262 |
case WL_RECHARGE_DOCKED: |
|
263 |
wl_charging_docked(source); |
|
264 |
break; |
|
265 |
default: |
|
266 |
STATION_DEBUG_PRINT("Error packet of unknown type received.\n"); |
|
267 |
break; |
|
268 |
} |
|
269 |
} |
|
270 |
|
|
271 |
/** |
|
272 |
* Called when we receive a packet alerting us that |
|
273 |
* a robot is looking for stations. |
|
274 |
* |
|
275 |
* @param source the robot requesting a station |
|
276 |
**/ |
|
277 |
void wl_charging_poll(int source) |
|
278 |
{ |
|
279 |
if (charging_is_space_available() && !charging_is_robot_seeking()) |
|
280 |
wl_charging_send_station_available(source); |
|
281 |
} |
|
282 |
|
|
283 |
/** |
|
284 |
* Called when we receive a request to charge. |
|
285 |
* |
|
286 |
* @param source the robot requesting to charge |
|
287 |
**/ |
|
288 |
void wl_charging_request(int source) |
|
289 |
{ |
|
290 |
if (charging_is_space_available() && !charging_is_robot_seeking()) |
|
291 |
{ |
|
292 |
charging_begin_seeking(source); |
|
293 |
wl_charging_send_request_accept(source); |
|
294 |
} |
|
295 |
else |
|
296 |
wl_charging_send_cancel(source); |
|
297 |
} |
|
298 |
|
|
299 |
/** |
|
300 |
* Called when we receive a request to verify our status |
|
301 |
* in the docking procedure. |
|
302 |
* |
|
303 |
* @param source the robot verifying our status |
|
304 |
**/ |
|
305 |
void wl_charging_verify(int source) |
|
306 |
{ |
|
307 |
int state = charging_get_robot_state(source); |
|
308 |
|
|
309 |
switch (state) |
|
310 |
{ |
|
311 |
case STATE_SEEKING: |
|
312 |
wl_charging_send_seeking(source); |
|
313 |
break; |
|
314 |
case STATE_DOCKED: |
|
315 |
wl_charging_send_docked(source); |
|
316 |
break; |
|
317 |
case STATE_NOT_CHARGING: |
|
318 |
STATION_DEBUG_PRINT("Received verify from unknown robot.\n"); |
|
319 |
wl_charging_send_cancel(source); |
|
320 |
break; |
|
321 |
default: |
|
322 |
STATION_DEBUG_PRINT("Unknown robot state.\n"); |
|
323 |
break; |
|
324 |
} |
|
325 |
} |
|
326 |
|
|
327 |
/** |
|
328 |
* Called when we receive a packet cancelling our |
|
329 |
* docking procedure. |
|
330 |
* |
|
331 |
* @param source the robot cancelling the procedure |
|
332 |
**/ |
|
333 |
void wl_charging_cancel(int source) |
|
334 |
{ |
|
335 |
int state = charging_get_robot_state(source); |
|
336 |
|
|
337 |
if (state == STATE_NOT_CHARGING) |
|
338 |
{ |
|
339 |
STATION_DEBUG_PRINT("Received cancel from unknown robot.\n"); |
|
340 |
return; |
|
341 |
} |
|
342 |
|
|
343 |
charging_set_verify_count(source, VERIFY_DELAY); |
|
344 |
|
|
345 |
charging_cancel(source); |
|
346 |
} |
|
347 |
|
|
348 |
/** |
|
349 |
* Called when we receive a packet stating that the |
|
350 |
* robot is seeking us. |
|
351 |
* |
|
352 |
* @param source the robot seeking us |
|
353 |
**/ |
|
354 |
void wl_charging_seeking(int source) |
|
355 |
{ |
|
356 |
int state = charging_get_robot_state(source); |
|
357 |
|
|
358 |
if (state == STATE_NOT_CHARGING) |
|
359 |
{ |
|
360 |
STATION_DEBUG_PRINT("Received seeking from unknown robot.\n"); |
|
361 |
wl_charging_send_cancel(source); |
|
362 |
return; |
|
363 |
} |
|
364 |
|
|
365 |
charging_set_verify_count(source, VERIFY_DELAY); |
|
366 |
|
|
367 |
if (state == STATE_DOCKED) |
|
368 |
{ |
|
369 |
STATION_DEBUG_PRINT("Should not have revert from docked to seeking.\n"); |
|
370 |
if (charging_is_robot_seeking()) |
|
371 |
{ |
|
372 |
charging_cancel(source); |
|
373 |
wl_charging_send_cancel(source); |
|
374 |
} |
|
375 |
else |
|
376 |
charging_begin_seeking(source); |
|
377 |
} |
|
378 |
} |
|
379 |
|
|
380 |
/** |
|
381 |
* Called when we receive a packet stating that |
|
382 |
* a robot is docked with us. |
|
383 |
* |
|
384 |
* @param source the robot that is docked with us |
|
385 |
**/ |
|
386 |
void wl_charging_docked(int source) |
|
387 |
{ |
|
388 |
int state = charging_get_robot_state(source); |
|
389 |
if (state == STATE_NOT_CHARGING) |
|
390 |
{ |
|
391 |
STATION_DEBUG_PRINT("Received docked from unknown robot.\n"); |
|
392 |
return; |
|
393 |
} |
|
394 |
|
|
395 |
charging_set_verify_count(source, VERIFY_DELAY); |
|
396 |
|
|
397 |
//seeking has ended |
|
398 |
if (state == STATE_SEEKING) |
|
399 |
charging_dock(source); |
|
400 |
} |
|
401 |
|
branches/autonomous_recharging/code/projects/autonomous_recharging/charging_station/charging_defs.h | ||
---|---|---|
1 |
/** |
|
2 |
* @file charging_defs.h |
|
3 |
* @brief Definitions for charging |
|
4 |
* |
|
5 |
* Contains definitions convenient for use in the charging station. |
|
6 |
* |
|
7 |
* @author Brian Coltin, Colony Project, CMU Robotics Club |
|
8 |
**/ |
|
9 |
|
|
10 |
//comment this out to not output debugging information |
|
11 |
#define STATION_DEBUG |
|
12 |
|
|
13 |
#ifdef STATION_DEBUG |
|
14 |
#define STATION_DEBUG_PRINT( s ) usb_puts( s ) |
|
15 |
#else |
|
16 |
#define STATION_DEBUG_PRINT( s ) |
|
17 |
#endif |
|
18 |
|
|
19 |
/** |
|
20 |
* @defgroup defs Charging Station Definitions |
|
21 |
* @brief Defines used in the charging station |
|
22 |
* |
|
23 |
* Definitions used in the charging station. |
|
24 |
* |
|
25 |
* @{ |
|
26 |
**/ |
|
27 |
|
|
28 |
/** @} **/ |
|
29 |
|
branches/autonomous_recharging/code/projects/autonomous_recharging/charging_station/wl_charging_station.h | ||
---|---|---|
1 |
/** |
|
2 |
* @file wl_charging_station.h |
|
3 |
* @brief A packet group for recharging messages |
|
4 |
* |
|
5 |
* A packet group for sending and receiving recharging |
|
6 |
* messages, implemented from the perspective of a charging |
|
7 |
* station. |
|
8 |
* |
|
9 |
* @author Brian Coltin, Colony Project, CMU Robotics Club |
|
10 |
**/ |
|
11 |
|
|
12 |
/** |
|
13 |
* @defgroup wlrecharge Recharging Packets |
|
14 |
* @brief Functions for sending and receiving recharging packets |
|
15 |
* |
|
16 |
* Functions for sending and receiving recharging packets. |
|
17 |
* |
|
18 |
* @{ |
|
19 |
**/ |
|
20 |
|
|
21 |
/**@brief Register this packet group with the wireless library **/ |
|
22 |
void wl_station_register(void); |
|
23 |
/**@brief Unregister this packet group with the wireless library **/ |
|
24 |
void wl_station_unregister(void); |
|
25 |
|
|
26 |
/** @} **/ // end defgroup |
|
27 |
|
branches/autonomous_recharging/code/projects/autonomous_recharging/charging_station/bays.c | ||
---|---|---|
125 | 125 |
return NUM_BAYS; |
126 | 126 |
} |
127 | 127 |
|
128 |
int assign_bay( int robot ) // 'smart' assign_bay() |
|
128 |
/*int assign_bay( int robot ) // 'smart' assign_bay()
|
|
129 | 129 |
{ |
130 | 130 |
int ldist = 0; |
131 | 131 |
int rdist = 0; |
... | ... | |
170 | 170 |
} |
171 | 171 |
bayStates[bay] = robot; |
172 | 172 |
return bay; |
173 |
} |
|
173 |
}*/
|
|
174 | 174 |
|
175 | 175 |
int robot_assignment(int robot) //returns bay to which 'robot' has been assigned, 255 if not yet assigned |
176 | 176 |
{ |
branches/autonomous_recharging/code/projects/autonomous_recharging/charging_station/charging.c | ||
---|---|---|
1 |
#include "charging.h" |
|
2 |
#include "charging_defs.h" |
|
3 |
|
|
4 |
#include <stdlib.h> |
|
5 |
#include <serial.h> |
|
6 |
|
|
7 |
#include <wl_token_ring.h> |
|
8 |
#include "wl_charging_station.h" |
|
9 |
|
|
10 |
//set this to the number of bays |
|
11 |
#define NUM_BAYS 2 |
|
12 |
|
|
13 |
//Function prototypes |
|
14 |
void charging_bom_on(void); |
|
15 |
void charging_bom_off(void); |
|
16 |
int charging_bom_read(void); |
|
17 |
|
|
18 |
//Structures |
|
19 |
typedef struct |
|
20 |
{ |
|
21 |
int state; |
|
22 |
int bay; |
|
23 |
int verifyCount; |
|
24 |
} ChargingRobot; |
|
25 |
|
|
26 |
ChargingRobot** robots; |
|
27 |
int robotsSize = 0; |
|
28 |
int numRobots = 0; |
|
29 |
int iteratorPos = 0; |
|
30 |
|
|
31 |
int bays[NUM_BAYS]; |
|
32 |
|
|
33 |
int is_seeking = 0; |
|
34 |
|
|
35 |
/** |
|
36 |
* Call this function to initialize charging. |
|
37 |
* wl_token_ring_register must be called before |
|
38 |
* this function. |
|
39 |
**/ |
|
40 |
void charging_init(void) |
|
41 |
{ |
|
42 |
robots = malloc(robotsSize * sizeof(ChargingRobot*)); |
|
43 |
if (!robots) |
|
44 |
{ |
|
45 |
STATION_DEBUG_PRINT("Out of memory.\n"); |
|
46 |
return; |
|
47 |
} |
|
48 |
|
|
49 |
int i; |
|
50 |
for (i = 0; i < robotsSize; i++) |
|
51 |
robots[i] = NULL; |
|
52 |
|
|
53 |
for (i = 0; i < NUM_BAYS; i++) |
|
54 |
bays[i] = -1; |
|
55 |
|
|
56 |
// call our own special function to set the BOM |
|
57 |
wl_token_ring_set_bom_functions(charging_bom_on, |
|
58 |
charging_bom_off, charging_bom_read); |
|
59 |
wl_station_register(); |
|
60 |
} |
|
61 |
|
|
62 |
/** |
|
63 |
* Initializes the iterator through the robots |
|
64 |
* which are using the charging station. |
|
65 |
**/ |
|
66 |
void charging_robot_iterator_init(void) |
|
67 |
{ |
|
68 |
iteratorPos = 0; |
|
69 |
} |
|
70 |
|
|
71 |
/** |
|
72 |
* Returns zero if the iterator has gone through |
|
73 |
* all robots that are using the charging station |
|
74 |
* since the last call to charging_robot_iterator_init, |
|
75 |
* nonzero otherwise. |
|
76 |
* |
|
77 |
* @return if the iterator has gone through all the robots |
|
78 |
**/ |
|
79 |
int charging_robot_iterator_has_next(void) |
|
80 |
{ |
|
81 |
while (iteratorPos < robotsSize) |
|
82 |
{ |
|
83 |
if (robots[iteratorPos] != NULL) |
|
84 |
return 1; |
|
85 |
iteratorPos++; |
|
86 |
} |
|
87 |
return 0; |
|
88 |
} |
|
89 |
|
|
90 |
/** |
|
91 |
* Returns the id of the next robot in the iterator |
|
92 |
* of robots that are using the charging station. |
|
93 |
* |
|
94 |
* @return the id of a robot using the station |
|
95 |
**/ |
|
96 |
int charging_robot_iterator_next(void) |
|
97 |
{ |
|
98 |
while (iteratorPos < robotsSize) |
|
99 |
{ |
|
100 |
if (robots[iteratorPos] != NULL) |
|
101 |
{ |
|
102 |
iteratorPos++; |
|
103 |
return iteratorPos - 1; |
|
104 |
} |
|
105 |
} |
|
106 |
|
|
107 |
return -1; |
|
108 |
} |
|
109 |
|
|
110 |
/** |
|
111 |
* Return the verification count for a robot |
|
112 |
* using the station. If the count is zero, |
|
113 |
* then the robot is probably dead. |
|
114 |
* |
|
115 |
* @param robot the id of the robot to check |
|
116 |
* |
|
117 |
* @return the verify count for robot robot |
|
118 |
**/ |
|
119 |
int charging_get_verify_count(int robot) |
|
120 |
{ |
|
121 |
if (robot >= robotsSize || robot < 0) |
|
122 |
{ |
|
123 |
STATION_DEBUG_PRINT("Index out of range.\n"); |
|
124 |
return -1; |
|
125 |
} |
|
126 |
if (robots[robot] == NULL) |
|
127 |
{ |
|
128 |
STATION_DEBUG_PRINT("Robot not known to charging station.\n"); |
|
129 |
return -1; |
|
130 |
} |
|
131 |
|
|
132 |
return robots[robot]->verifyCount; |
|
133 |
} |
|
134 |
|
|
135 |
/** |
|
136 |
* Sets the verify count for the specified robot. |
|
137 |
* |
|
138 |
* @param robot the robot to specify the count for |
|
139 |
* @param count the new verify count |
|
140 |
**/ |
|
141 |
void charging_set_verify_count(int robot, int count) |
|
142 |
{ |
|
143 |
if (robot >= robotsSize || robot < 0) |
|
144 |
{ |
|
145 |
STATION_DEBUG_PRINT("Index out of range.\n"); |
|
146 |
return; |
|
147 |
} |
|
148 |
if (robots[robot] == NULL) |
|
149 |
{ |
|
150 |
STATION_DEBUG_PRINT("Robot not known to charging station.\n"); |
|
151 |
return; |
|
152 |
} |
|
153 |
|
|
154 |
robots[robot]->verifyCount = count; |
|
155 |
} |
|
156 |
|
|
157 |
/** |
|
158 |
* Called when charging has been cancelled for a |
|
159 |
* specific robot. |
|
160 |
* |
|
161 |
* @param robot the robot to cancel charging for |
|
162 |
**/ |
|
163 |
void charging_cancel(int robot) |
|
164 |
{ |
|
165 |
if (robot < 0 || robot >= robotsSize || robots[robot] == NULL) |
|
166 |
{ |
|
167 |
STATION_DEBUG_PRINT("Attempted to cancel charging for an "); |
|
168 |
STATION_DEBUG_PRINT("unknown robot.\n"); |
|
169 |
return; |
|
170 |
} |
|
171 |
|
|
172 |
//allow other robots to seek |
|
173 |
if (robots[robot]->state == STATE_SEEKING) |
|
174 |
{ |
|
175 |
//TODO : deactivate BOMs and homing beacons |
|
176 |
is_seeking = 0; |
|
177 |
} |
|
178 |
|
|
179 |
//free the bay |
|
180 |
bays[robots[robot]->bay] = -1; |
|
181 |
|
|
182 |
free(robots[robot]); |
|
183 |
robots[robot] = NULL; |
|
184 |
numRobots--; |
|
185 |
} |
|
186 |
|
|
187 |
/** |
|
188 |
* Assigns a bay to a specific robot. |
|
189 |
* |
|
190 |
* @param robot the robot to assign the bay to |
|
191 |
* |
|
192 |
* @return the identifier for the assigned bay |
|
193 |
**/ |
|
194 |
int assign_bay(int robot) |
|
195 |
{ |
|
196 |
int bay = 0; |
|
197 |
|
|
198 |
//TODO: implement |
|
199 |
|
|
200 |
bays[bay] = robot; |
|
201 |
return bay; |
|
202 |
} |
|
203 |
|
|
204 |
/** |
|
205 |
* Called when a robot begins seeking. |
|
206 |
* |
|
207 |
* @param robot the robot which has begun seeking |
|
208 |
**/ |
|
209 |
void charging_begin_seeking(int robot) |
|
210 |
{ |
|
211 |
if (robot > robotsSize) |
|
212 |
{ |
|
213 |
int nextSize = robot + 1; |
|
214 |
ChargingRobot** temp = malloc(nextSize * sizeof(ChargingRobot*)); |
|
215 |
if (temp == NULL) |
|
216 |
{ |
|
217 |
STATION_DEBUG_PRINT("Out of memory.\n"); |
|
218 |
return; |
|
219 |
} |
|
220 |
int i; |
|
221 |
for (i = 0; i < robotsSize; i++) |
|
222 |
temp[i] = robots[i]; |
|
223 |
for (; i < nextSize; i++) |
|
224 |
temp[i] = NULL; |
|
225 |
free(robots); |
|
226 |
robots = temp; |
|
227 |
robotsSize = nextSize; |
|
228 |
} |
|
229 |
|
|
230 |
if (charging_is_robot_seeking()) |
|
231 |
{ |
|
232 |
STATION_DEBUG_PRINT("A robot is already seeking.\n"); |
|
233 |
return; |
|
234 |
} |
|
235 |
|
|
236 |
//check if robot has reverted from docking to seeking |
|
237 |
if (robots[robot] != NULL) |
|
238 |
{ |
|
239 |
switch (robots[robot]->state) |
|
240 |
{ |
|
241 |
case STATE_SEEKING: |
|
242 |
//do nothing |
|
243 |
STATION_DEBUG_PRINT("Robot is already "); |
|
244 |
STATION_DEBUG_PRINT("seeking.\n"); |
|
245 |
break; |
|
246 |
case STATE_DOCKED: |
|
247 |
robots[robot]->state = STATE_SEEKING; |
|
248 |
is_seeking = 1; |
|
249 |
//TODO: active robots[robot]->bay |
|
250 |
break; |
|
251 |
default: |
|
252 |
STATION_DEBUG_PRINT("Robot is in an "); |
|
253 |
STATION_DEBUG_PRINT("unexpected state.\n"); |
|
254 |
break; |
|
255 |
} |
|
256 |
return; |
|
257 |
} |
|
258 |
|
|
259 |
if (!charging_is_space_available()) |
|
260 |
{ |
|
261 |
STATION_DEBUG_PRINT("No bays are available for charging.\n"); |
|
262 |
return; |
|
263 |
} |
|
264 |
|
|
265 |
ChargingRobot* r = malloc(sizeof(ChargingRobot)); |
|
266 |
if (!r) |
|
267 |
{ |
|
268 |
STATION_DEBUG_PRINT("Out of memory.\n"); |
|
269 |
return; |
|
270 |
} |
|
271 |
|
|
272 |
r->state = STATE_SEEKING; |
|
273 |
r->bay = assign_bay(robot); |
|
274 |
r->verifyCount = 0; |
|
275 |
robots[robot] = r; |
|
276 |
numRobots++; |
|
277 |
is_seeking = 1; |
|
278 |
//TODO: activate r->bay |
|
279 |
} |
|
280 |
|
|
281 |
/** |
|
282 |
* Called when a robot has docked. |
|
283 |
* |
|
284 |
* @param robot the robot which has docked |
|
285 |
**/ |
|
286 |
void charging_dock(int robot) |
|
287 |
{ |
|
288 |
if (robots[robot] == NULL) |
|
289 |
{ |
|
290 |
STATION_DEBUG_PRINT("Should not proceed from not "); |
|
291 |
STATION_DEBUG_PRINT("charging to docked.\n"); |
|
292 |
return; |
|
293 |
} |
|
294 |
|
|
295 |
if (robots[robot]->state == STATE_DOCKED) |
|
296 |
{ |
|
297 |
STATION_DEBUG_PRINT("Robot is already docked.\n"); |
|
298 |
return; |
|
299 |
} |
|
300 |
|
|
301 |
//robot was seeking before |
|
302 |
is_seeking = 0; |
|
303 |
//TODO: deactivate boms and homing beacons |
|
304 |
robots[robot]->state = STATE_DOCKED; |
|
305 |
} |
|
306 |
|
|
307 |
/** |
|
308 |
* Checks if there are any available bays |
|
309 |
* for a robot to occupy. |
|
310 |
* |
|
311 |
* @return nonzero if there are bays available, |
|
312 |
* and zero otherwise. |
|
313 |
**/ |
|
314 |
int charging_is_space_available(void) |
|
315 |
{ |
|
316 |
return numRobots < NUM_BAYS; |
|
317 |
} |
|
318 |
|
|
319 |
/** |
|
320 |
* Checks if there is a robot which |
|
321 |
* is currently seeking the station. |
|
322 |
* |
|
323 |
* @return nonzero if a robot is seeking the |
|
324 |
* station, zero otherwise. |
|
325 |
**/ |
|
326 |
int charging_is_robot_seeking(void) |
|
327 |
{ |
|
328 |
return is_seeking; |
|
329 |
} |
|
330 |
|
|
331 |
/** |
|
332 |
* Returns the state of the specified robot. |
|
333 |
* |
|
334 |
* @param robot the robot to get the state of |
|
335 |
* |
|
336 |
* @return either STATE_NOT_CHARGING, STATE_SEEKING, or STATE_DOCKED |
|
337 |
**/ |
|
338 |
int charging_get_robot_state(int robot) |
|
339 |
{ |
|
340 |
if (robot >= robotsSize || robots[robot] == NULL) |
|
341 |
return STATE_NOT_CHARGING; |
|
342 |
return robots[robot]->state; |
|
343 |
} |
|
344 |
|
|
345 |
/** |
|
346 |
* Called when the BOM needs to be turned on for the |
|
347 |
* token ring. If a robot is seeking, only the BOM of |
|
348 |
* the bay that is being sought will turn on. If no |
|
349 |
* robot is seeking, the BOM for all of the bays will |
|
350 |
* turn on. |
|
351 |
**/ |
|
352 |
void charging_bom_on(void) |
|
353 |
{ |
|
354 |
//TODO: implement |
|
355 |
} |
|
356 |
|
|
357 |
/** |
|
358 |
* Called when the BOM needs to be turned off for the |
|
359 |
* token ring. Turns off the BOM for all bays. |
|
360 |
**/ |
|
361 |
void charging_bom_off(void) |
|
362 |
{ |
|
363 |
//TODO: implement |
|
364 |
} |
|
365 |
|
|
366 |
/** |
|
367 |
* The charging station BOM can only emit, so |
|
368 |
* we just return -1. |
|
369 |
**/ |
|
370 |
int charging_bom_read(void) |
|
371 |
{ |
|
372 |
return -1; |
|
373 |
} |
|
374 |
|
branches/autonomous_recharging/code/projects/autonomous_recharging/charging_station/main.c | ||
---|---|---|
1 |
#include <firefly+_lib.h> |
|
1 | 2 |
|
2 |
#include "firefly+_lib.h" |
|
3 |
|
|
4 | 3 |
#include <wireless.h> |
5 | 4 |
#include <wl_token_ring.h> |
6 | 5 |
|
7 |
#include "bays.h"
|
|
6 |
#include "charging.h"
|
|
8 | 7 |
|
9 |
int main(void) { |
|
10 |
|
|
11 |
//lcd, orb, all that good stuff |
|
8 |
int main(void) |
|
9 |
{ |
|
10 |
analog_init(); |
|
11 |
led_init(); |
|
12 |
orb_init(); |
|
13 |
buzzer_init(); |
|
14 |
usb_init(BAUD115200); |
|
15 |
xbee_init(BAUD9600); |
|
12 | 16 |
|
13 |
analog_init(); |
|
14 |
led_init(); |
|
15 |
orb_init(); |
|
16 |
buzzer_init(); |
|
17 |
|
|
18 |
usb_init(BAUD115200); |
|
19 |
xbee_init(BAUD9600); |
|
20 |
|
|
21 |
usb_puts("Beginning.\n"); |
|
22 |
wl_init(); |
|
23 |
usb_puts("Done initializing.\n"); |
|
24 |
wl_token_ring_register(); |
|
25 |
wl_token_ring_join(); |
|
26 |
bays_init(); |
|
27 |
|
|
28 |
orb_set_color(GREEN); |
|
29 |
|
|
30 |
lbom_off(); |
|
31 |
|
|
32 |
while(1) |
|
33 |
{ |
|
34 |
//this needs to happen continuously -- it handles the packet |
|
17 |
//dragonfly_init(ALL_ON); |
|
18 |
|
|
19 |
wl_init(); |
|
20 |
wl_token_ring_register(); |
|
21 |
charging_init(); |
|
22 |
|
|
23 |
while (1) |
|
35 | 24 |
wl_do(); |
36 |
do_consistency_checks(); |
|
37 |
|
|
38 |
if(button1_click()) |
|
39 |
{ |
|
40 |
print_bays_debug(); |
|
41 |
} |
|
42 |
if(button2_click()) |
|
43 |
{ |
|
44 |
if(bayStates[1] == 255) |
|
45 |
{ |
|
46 |
bayStates[1] = 50; |
|
47 |
usb_puts("\n**filled bay 1 with bogus robot (50)**\n"); |
|
48 |
print_bays_debug(); |
|
49 |
orb_set_color(RED); |
|
50 |
} |
|
51 |
else if(bayStates[1] == 50) |
|
52 |
{ |
|
53 |
bayStates[1] = 255; |
|
54 |
usb_puts("\n**freed bogus robot (50) from bay 1 bay 1**\n"); |
|
55 |
print_bays_debug(); |
|
56 |
orb_set_color(BLUE); |
|
57 |
} |
|
58 |
else |
|
59 |
{ |
|
60 |
usb_puts("\n**bay 1 already in use by legitimate robot**\n"); |
|
61 |
print_bays_debug(); |
|
62 |
orb_set_color(GREEN); |
|
63 |
} |
|
64 |
} |
|
65 |
} |
|
66 |
return 0; |
|
25 |
|
|
26 |
return 0; |
|
67 | 27 |
} |
28 |
|
branches/autonomous_recharging/code/projects/autonomous_recharging/charging_station/charging.h | ||
---|---|---|
1 |
/** |
|
2 |
* @file charging.h Charging |
|
3 |
* |
|
4 |
* @brief Charging station functions |
|
5 |
* |
|
6 |
* Contains functions for the charging station, controlling |
|
7 |
* bay management, homing beacons and BOMS. |
|
8 |
**/ |
|
9 |
|
|
10 |
/** |
|
11 |
* @defgroup charging Charging Station |
|
12 |
* |
|
13 |
* @brief Charging Station Functionality |
|
14 |
* |
|
15 |
* Contains functions for the charging station, |
|
16 |
* controlling bay management, the homing beacons |
|
17 |
* and BOMS. |
|
18 |
* |
|
19 |
* @{ |
|
20 |
**/ |
|
21 |
|
|
22 |
//possible robot states |
|
23 |
#define STATE_NOT_CHARGING 1 |
|
24 |
#define STATE_SEEKING 2 |
|
25 |
#define STATE_DOCKED 3 |
|
26 |
|
|
27 |
/** @brief Initialize charging **/ |
|
28 |
void charging_init(void); |
|
29 |
|
|
30 |
/** @brief Initialize an iterator through the robots **/ |
|
31 |
void charging_robot_iterator_init(void); |
|
32 |
/** @brief Check if there is another robot to iterate through **/ |
|
33 |
int charging_robot_iterator_has_next(void); |
|
34 |
/** @brief Get the next robot's id **/ |
|
35 |
int charging_robot_iterator_next(void); |
|
36 |
|
|
37 |
/** @brief Get the verify count for a robot **/ |
|
38 |
int charging_get_verify_count(int robot); |
|
39 |
/** @brief Set the verify count for a robot **/ |
|
40 |
void charging_set_verify_count(int robot, int count); |
|
41 |
|
|
42 |
/** @brief Cancel charging for a robot **/ |
|
43 |
void charging_cancel(int robot); |
|
44 |
/** @brief Begin seeking for a robot **/ |
|
45 |
void charging_begin_seeking(int robot); |
|
46 |
/** @brief Dock a robot with the charging station **/ |
|
47 |
void charging_dock(int robot); |
|
48 |
|
|
49 |
/** @brief Check if room is available to charge **/ |
|
50 |
int charging_is_space_available(void); |
|
51 |
/** @brief Check if a robot is currently seeking **/ |
|
52 |
int charging_is_robot_seeking(void); |
|
53 |
/** @brief Get the supposed state of a robot **/ |
|
54 |
int charging_get_robot_state(int robot); |
|
55 |
|
|
56 |
/** @} **/ |
|
57 |
|
Also available in: Unified diff