root / branches / autonomous_recharging / code / projects / autonomous_recharging / charging_station / charging.c @ 98
History | View | Annotate | Download (7.08 KB)
1 | 98 | bcoltin | #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 | } |