Project

General

Profile

Statistics
| Revision:

root / branches / autonomous_recharging / code / projects / autonomous_recharging / charging_station / charging.c @ 98

History | View | Annotate | Download (7.08 KB)

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