Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (8.57 KB)

1
#include "charging.h"
2
#include "charging_defs.h"
3
#include "bays.h"
4

    
5
#include <stdlib.h>
6
#include <serial.h>
7

    
8
#include <wl_token_ring.h>
9
#include "wl_charging_station.h"
10

    
11
//set this to the number of bays
12
#define NUM_BAYS 2
13

    
14
//Function prototypes
15
void charging_bom_on(void);
16
void charging_bom_off(void);
17
int charging_bom_read(void);
18

    
19
//Structures
20
typedef struct
21
{
22
        int state;
23
        int bay;
24
        int verifyCount;
25
        int battery;
26
} ChargingRobot;
27

    
28
ChargingRobot** robots;
29
int robotsSize = 0;
30
int numRobots = 0;
31
int iteratorPos = 0;
32

    
33
int bays[NUM_BAYS];
34

    
35
int is_seeking = 0;
36

    
37
/**
38
 * Call this function to initialize charging.
39
 * wl_token_ring_register must be called before
40
 * this function.
41
 **/
42
void charging_init(void)
43
{
44
        robots = malloc(robotsSize * sizeof(ChargingRobot*));
45
        if (!robots)
46
        {
47
                STATION_DEBUG_PRINT("Out of memory.\n");
48
                return;
49
        }
50
        
51
        int i;
52
        for (i = 0; i < robotsSize; i++)
53
                robots[i] = NULL;
54

    
55
        for (i = 0; i < NUM_BAYS; i++)
56
                bays[i] = -1;
57

    
58
        bays_init();
59

    
60
        // call our own special function to set the BOM
61
        wl_token_ring_set_bom_functions(charging_bom_on,
62
                charging_bom_off, charging_bom_read);
63
        wl_station_register();
64
}
65

    
66
/**
67
 * Initializes the iterator through the robots
68
 * which are using the charging station.
69
 **/
70
void charging_robot_iterator_init(void)
71
{
72
        iteratorPos = 0;
73
}
74

    
75
/**
76
 * Returns zero if the iterator has gone through
77
 * all robots that are using the charging station
78
 * since the last call to charging_robot_iterator_init,
79
 * nonzero otherwise.
80
 *
81
 * @return if the iterator has gone through all the robots
82
 **/
83
int charging_robot_iterator_has_next(void)
84
{
85
        while (iteratorPos < robotsSize)
86
        {
87
                if (robots[iteratorPos] != NULL)
88
                        return 1;
89
                iteratorPos++;
90
        }
91
        return 0;
92
}
93

    
94
/**
95
 * Returns the id of the next robot in the iterator
96
 * of robots that are using the charging station.
97
 *
98
 * @return the id of a robot using the station
99
 **/
100
int charging_robot_iterator_next(void)
101
{
102
        while (iteratorPos < robotsSize)
103
        {
104
                if (robots[iteratorPos] != NULL)
105
                {
106
                        iteratorPos++;
107
                        return iteratorPos - 1;
108
                }
109
        }
110
        
111
        return -1;
112
}
113

    
114
/**
115
 * Return the verification count for a robot
116
 * using the station. If the count is zero,
117
 * then the robot is probably dead.
118
 *
119
 * @param robot the id of the robot to check
120
 * 
121
 * @return the verify count for robot robot
122
 **/
123
int charging_get_verify_count(int robot)
124
{
125
        if (robot >= robotsSize || robot < 0)
126
        {
127
                STATION_DEBUG_PRINT("Index out of range.\n");
128
                return -1;
129
        }
130
        if (robots[robot] == NULL)
131
        {
132
                STATION_DEBUG_PRINT("Robot not known to charging station.\n");
133
                return -1;
134
        }
135

    
136
        return robots[robot]->verifyCount;
137
}
138

    
139
/**
140
 * Sets the verify count for the specified robot.
141
 *
142
 * @param robot the robot to specify the count for
143
 * @param count the new verify count
144
 **/
145
void charging_set_verify_count(int robot, int count)
146
{
147
        if (robot >= robotsSize || robot < 0)
148
        {
149
                STATION_DEBUG_PRINT("Index out of range.\n");
150
                return;
151
        }
152
        if (robots[robot] == NULL)
153
        {
154
                STATION_DEBUG_PRINT("Robot not known to charging station.\n");
155
                return;
156
        }
157

    
158
        robots[robot]->verifyCount = count;
159
}
160

    
161
/**
162
 * Called when charging has been cancelled for a 
163
 * specific robot.
164
 *
165
 * @param robot the robot to cancel charging for
166
 **/
167
void charging_cancel(int robot)
168
{
169
        if (robot < 0 || robot >= robotsSize || robots[robot] == NULL)
170
        {
171
                STATION_DEBUG_PRINT("Attempted to cancel charging for an ");
172
                STATION_DEBUG_PRINT("unknown robot.\n");
173
                return;
174
        }
175

    
176
        STATION_DEBUG_PRINT("Cancelling charging for robot ");
177
        STATION_DEBUG_PUTI(robot);
178
        STATION_DEBUG_PRINT(".\n");
179

    
180
        //allow other robots to seek
181
        if (robots[robot]->state == STATE_SEEKING)
182
        {
183
                bay_disable();
184
                is_seeking = 0;
185
        }
186

    
187
        //free the bay
188
        bays[robots[robot]->bay] = -1;
189

    
190
        free(robots[robot]);
191
        robots[robot] = NULL;
192
        numRobots--;
193
}
194

    
195
/**
196
 * Assigns a bay to a specific robot.
197
 *
198
 * @param robot the robot to assign the bay to
199
 *
200
 * @return the identifier for the assigned bay
201
 **/
202
int assign_bay(int robot)
203
{
204
        int bay = 0;
205

    
206
        //TODO: use better algorithm based on distance
207
        for (bay = 0; bay < NUM_BAYS; bay++)
208
                if (bays[bay] == -1)
209
                        break;
210

    
211
        if (bay == NUM_BAYS)
212
        {
213
                STATION_DEBUG_PRINT("Attempted to assign a bay when all ");
214
                STATION_DEBUG_PRINT("are full.\n");
215
        }
216

    
217
        bays[bay] = robot;
218
        return bay;
219
}
220

    
221
/**
222
 * Called when a robot begins seeking.
223
 *
224
 * @param robot the robot which has begun seeking
225
 **/
226
void charging_begin_seeking(int robot)
227
{
228
        STATION_DEBUG_PRINT("Robot ");
229
        STATION_DEBUG_PUTI(robot);
230
        STATION_DEBUG_PRINT(" has begun seeking.\n");
231
        
232
        if (robot > robotsSize)
233
        {
234
                int nextSize = robot + 1;
235
                ChargingRobot** temp = malloc(nextSize * sizeof(ChargingRobot*));
236
                if (temp == NULL)
237
                {
238
                        STATION_DEBUG_PRINT("Out of memory.\n");
239
                        return;
240
                }
241
                int i;
242
                for (i = 0; i < robotsSize; i++)
243
                        temp[i] = robots[i];
244
                for (; i < nextSize; i++)
245
                        temp[i] = NULL;
246
                free(robots);
247
                robots = temp;
248
                robotsSize = nextSize;
249
        }
250

    
251
        if (charging_is_robot_seeking())
252
        {
253
                STATION_DEBUG_PRINT("A robot is already seeking.\n");
254
                return;
255
        }
256

    
257
        //check if robot has reverted from docking to seeking
258
        if (robots[robot] != NULL)
259
        {
260
                switch (robots[robot]->state)
261
                {
262
                        case STATE_SEEKING:
263
                                //do nothing
264
                                STATION_DEBUG_PRINT("Robot is already ");
265
                                STATION_DEBUG_PRINT("seeking.\n");
266
                                break;
267
                        case STATE_DOCKED:
268
                                if (is_seeking)
269
                                {
270
                                        //we can only allow one robot to seek
271
                                        robots[robot]->state = STATE_NOT_CHARGING;
272
                                        bays[robots[robot]->bay] = -1;
273
                                        robots[robot]->bay = -1;
274
                                        break;
275
                                }
276
                                robots[robot]->state = STATE_SEEKING;
277
                                is_seeking = 1;
278
                                bay_enable(robots[robot]->bay);
279
                                break;
280
                        default:
281
                                STATION_DEBUG_PRINT("Robot is in an ");
282
                                STATION_DEBUG_PRINT("unexpected state.\n");
283
                                break;
284
                }
285
                return;
286
        }
287

    
288
        if (!charging_is_space_available())
289
        {
290
                STATION_DEBUG_PRINT("No bays are available for charging.\n");
291
                return;
292
        }
293

    
294
        ChargingRobot* r = malloc(sizeof(ChargingRobot));
295
        if (!r)
296
        {
297
                STATION_DEBUG_PRINT("Out of memory.\n");
298
                return;
299
        }
300

    
301
        r->state = STATE_SEEKING;
302
        r->bay = assign_bay(robot);
303
        r->verifyCount = VERIFY_DELAY;
304
        r->battery = 0;
305
        robots[robot] = r;
306
        numRobots++;
307
        is_seeking = 1;
308
        bay_enable(r->bay);
309
}
310

    
311
/**
312
 * Called when a robot has docked.
313
 *
314
 * @param robot the robot which has docked
315
 **/
316
void charging_dock(int robot)
317
{
318
        STATION_DEBUG_PRINT("Robot ");
319
        STATION_DEBUG_PUTI(robot);
320
        STATION_DEBUG_PRINT(" has docked with the station.\n");
321

    
322
        if (robots[robot] == NULL)
323
        {
324
                STATION_DEBUG_PRINT("Should not proceed from not ");
325
                STATION_DEBUG_PRINT("charging to docked.\n");
326
                return;
327
        }
328

    
329
        if (robots[robot]->state == STATE_DOCKED)
330
        {
331
                STATION_DEBUG_PRINT("Robot is already docked.\n");
332
                return;
333
        }
334

    
335
        //robot was seeking before
336
        is_seeking = 0;
337
        bay_disable();
338
        robots[robot]->state = STATE_DOCKED;
339
}
340

    
341
/**
342
 * Checks if there are any available bays
343
 * for a robot to occupy.
344
 *
345
 * @return nonzero if there are bays available,
346
 * and zero otherwise.
347
 **/
348
int charging_is_space_available(void)
349
{
350
        return numRobots < NUM_BAYS;
351
}
352

    
353
/**
354
 * Checks if there is a robot which
355
 * is currently seeking the station.
356
 *
357
 * @return nonzero if a robot is seeking the
358
 * station, zero otherwise.
359
 **/
360
int charging_is_robot_seeking(void)
361
{
362
        return is_seeking;
363
}
364

    
365
/**
366
 * Returns the state of the specified robot.
367
 *
368
 * @param robot the robot to get the state of
369
 *
370
 * @return either STATE_NOT_CHARGING, STATE_SEEKING, or STATE_DOCKED
371
 **/
372
int charging_get_robot_state(int robot)
373
{
374
        if (robot >= robotsSize || robots[robot] == NULL)
375
                return STATE_NOT_CHARGING;
376
        return robots[robot]->state;
377
}
378

    
379
/**
380
 * Update battery reading of a robot
381
 *
382
 * @param robot the robot to update battery reading
383
 **/
384
void update_battery(int robot, int battery)
385
{
386
        if (robot < robotSize && robots[robot] != NULL)
387
                robots[robot]->battery = battery;
388
}
389
void check_battery(int robot, int battery)
390
{
391
        if (charging_is_space_available()) return;
392
        // 152 low voltage
393
        // 179 charging voltage
394
        // there's no way to tell the voltage of a charging robot, so we just eject the first one
395
        charging_robot_iterator_init();
396
        int r = charging_robot_iterator_next();
397
        int b = robots[r]->battery;
398
        if (battery<153){
399
                //cancel robot r
400
                wl_charging_send_cancel(r);
401
                charging_cancel(r);
402
                //tell robot that charging station is now available
403
                wl_charging_send_station_available(robot);
404
        }
405
}
406

    
407
/**
408
 * Called when the BOM needs to be turned on for the 
409
 * token ring. If a robot is seeking, only the BOM of
410
 * the bay that is being sought will turn on. If no
411
 * robot is seeking, the BOM for all of the bays will
412
 * turn on.
413
 **/
414
void charging_bom_on(void)
415
{
416
        lbom_on();
417
}
418

    
419
/**
420
 * Called when the BOM needs to be turned off for the
421
 * token ring. Turns off the BOM for all bays.
422
 **/
423
void charging_bom_off(void)
424
{
425
        lbom_off();
426
}
427

    
428
/**
429
 * The charging station BOM can only emit, so
430
 * we just return -1.
431
 **/
432
int charging_bom_read(void)
433
{
434
        return -1;
435
}
436