Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (10.6 KB)

1
#include "wl_charging_station.h"
2

    
3
#include <wireless.h>
4
#include <wl_defs.h>
5
#include <serial.h>
6

    
7
#include "charging_defs.h"
8
#include "charging.h"
9

    
10
#define NULL 0
11

    
12
//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
void wl_charging_send_battery(int dest);
49

    
50
// the handler
51
PacketGroupHandler wl_charging_handler =
52
                {WL_RECHARGE_GROUP, wl_charging_timeout_handler,
53
                wl_charging_response_handler, wl_charging_handle_receive,
54
                wl_charging_cleanup};
55

    
56

    
57
//frames
58
#define STATION_AVAILABLE_FRAME 1
59
#define REQUEST_ACCEPT_FRAME 2
60
#define VERIFY_FRAME 3
61
#define CANCEL_FRAME 4
62
#define SEEKING_FRAME 5
63
#define DOCKED_FRAME 6
64
#define BATTERY_FRAME 7
65

    
66
/**
67
 * Register this packet group with the wireless library.
68
 * This function must be called before any other wl_charging
69
 * function. Also, wl_token_ring_register must be called
70
 * before this function, along with wl_token_ring_join.
71
 *
72
 * @see wl_charging_unregister
73
 **/
74
void wl_station_register(void)
75
{
76
        wl_register_packet_group(&wl_charging_handler);
77
}
78

    
79
/**
80
 * Called to unregister this packet group with the wireless
81
 * library.
82
 *
83
 * @see wl_charging_register
84
 **/
85
void wl_station_unregister(void)
86
{
87
        wl_unregister_packet_group(&wl_charging_handler);
88
}
89

    
90
/**
91
 * Sends a packet alerting a specific robot that the station
92
 * is available.
93
 *
94
 * @param dest the robot to tell that the station is available
95
 **/
96
void wl_charging_send_station_available(int dest)
97
{
98
        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP,
99
                WL_RECHARGE_STATION_AVAILABLE, NULL, 0, dest,
100
                STATION_AVAILABLE_FRAME);
101
}
102

    
103
/**
104
 * Sends a packet accepting the request of a specific robot.
105
 *
106
 * @param dest the robot to alert of its acceptance
107
 **/
108
void wl_charging_send_request_accept(int dest)
109
{
110
        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP,
111
                WL_RECHARGE_REQUEST_ACCEPT, NULL, 0, dest,
112
                REQUEST_ACCEPT_FRAME);
113
}
114

    
115
/**
116
 * Sends a request to verify what the robot
117
 * believes it is doing.
118
 *
119
 * @param dest the robot to check
120
 **/
121
void wl_charging_send_verify(int dest)
122
{
123
        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_VERIFY,
124
                                NULL, 0, dest, VERIFY_FRAME);
125
}
126

    
127
/**
128
 * Sends a packet to the robot that we are
129
 * no longer allowing it to charge.
130
 *
131
 * @param dest the robot to tell that it cannot charge
132
 **/
133
void wl_charging_send_cancel(int dest)
134
{
135
        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_CANCEL,
136
                                NULL, 0, dest, CANCEL_FRAME);
137
}
138

    
139
/**
140
 * Sends a packet to the robot that it may
141
 * attempt to dock with us.
142
 * 
143
 * @param dest the robot attempting to dock
144
 **/
145
void wl_charging_send_seeking(int dest)
146
{
147
        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_SEEKING,
148
                                NULL, 0, dest, SEEKING_FRAME);
149
}
150

    
151
/**
152
 * Sends a packet to the robot that
153
 * we believe it is docked with us.
154
 *
155
 * @param dest the robot that has docked with us
156
 **/
157
void wl_charging_send_docked(int dest)
158
{
159
        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_DOCKED,
160
                                NULL, 0, dest, DOCKED_FRAME);
161
}
162

    
163
/**
164
 * Sends a packet to request for the robot's battery reading
165
 *
166
 * @param dest the robot to request
167
 **/
168
void wl_charging_send_battery(int dest)
169
{
170
        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_BATTERY, NULL, 0, dest, BATTERY_FRAME);
171
}
172

    
173
/**
174
 * Called when this packet is unregistered with the wireless library.
175
 **/
176
void wl_charging_cleanup(void)
177
{
178

    
179
}
180

    
181
/**
182
 * Called when the timer goes off in the wireless library.
183
 **/
184
void wl_charging_timeout_handler(void)
185
{
186
        charging_robot_iterator_init();
187
        
188
        while (charging_robot_iterator_has_next())
189
        {
190
                int robot = charging_robot_iterator_next();
191

    
192
                int c = charging_get_verify_count(robot);
193
                if (c <= 0)
194
                {
195
                        STATION_DEBUG_PRINT("Robot ");
196
                        STATION_DEBUG_PUTI(robot);
197
                        STATION_DEBUG_PRINT(" has timed out.");
198
                
199
                        charging_cancel(robot);
200
                        continue;
201
                }
202
        
203
                if (c % VERIFY_FREQUENCY == 0 && c != VERIFY_DELAY)
204
                {
205
                        STATION_DEBUG_PRINT("Verification sent to robot ");
206
                        STATION_DEBUG_PUTI(robot);
207
                        STATION_DEBUG_PRINT(".\n");
208
                        wl_charging_send_verify(robot);
209
                }
210
                
211
                charging_set_verify_count(robot, c - 1);
212
        }
213
}
214

    
215
/**
216
 * Called when a response is received regarding a packet that was sent.
217
 * 
218
 * @param frame the frame number for the packet
219
 * @param received true if the packet was received, false otherwise
220
 **/
221
void wl_charging_response_handler(int frame, int received)
222
{
223
        if (received)
224
                return;
225
        if (!received)
226
        {
227
                STATION_DEBUG_PRINT("Sent packet not recieved.\n");
228
                switch (frame)
229
                {
230
                        case STATION_AVAILABLE_FRAME:
231
                                break;
232
                        case REQUEST_ACCEPT_FRAME:
233
                                break;
234
                        case VERIFY_FRAME:
235
                                break;
236
                        case CANCEL_FRAME:
237
                        case SEEKING_FRAME:
238
                        case DOCKED_FRAME:
239
                                break;
240
                }
241
        }
242
}
243

    
244
/**
245
 * Handles receiving an error packet.
246
 *
247
 * @param type the packet type
248
 * @param source the 16-bit address of the packet's sender
249
 * @param packet the packet data
250
 * @param length the length in bytes of the packet
251
 **/
252
void wl_charging_handle_receive(char type, int source, unsigned char* packet,
253
                                                        int length)
254
{
255
        switch (type)
256
        {
257
                case WL_RECHARGE_POLL_STATIONS:
258
                        wl_charging_poll(source);
259
                        break;
260
                case WL_RECHARGE_STATION_AVAILABLE:
261
                        STATION_DEBUG_PRINT("Stations should not receive ");
262
                        STATION_DEBUG_PRINT("station available packets.\n");
263
                        break;
264
                case WL_RECHARGE_REQUEST:
265
                        wl_charging_request(source);
266
                        break;
267
                case WL_RECHARGE_REQUEST_ACCEPT:
268
                        STATION_DEBUG_PRINT("Stations should not receive ");
269
                        STATION_DEBUG_PRINT("request accept packets.\n");
270
                        break;
271
                case WL_RECHARGE_VERIFY:
272
                        wl_charging_verify(source);
273
                        break;
274
                case WL_RECHARGE_CANCEL:
275
                        wl_charging_cancel(source);
276
                        break;
277
                case WL_RECHARGE_SEEKING:
278
                        wl_charging_seeking(source);
279
                        break;
280
                case WL_RECHARGE_DOCKED:
281
                        wl_charging_docked(source);
282
                        break;
283
                case WL_RECHARGE_BATTERY:
284
                        wl_charging_battery(source,packet,length);
285
                default:
286
                        STATION_DEBUG_PRINT("Error packet of unknown type received.\n");
287
                        break;
288
        }
289
}
290

    
291
/**
292
 * Called when we receive a packet alerting us that
293
 * a robot is looking for stations.
294
 *
295
 * @param source the robot requesting a station
296
 **/
297
void wl_charging_poll(int source)
298
{
299
        if (charging_is_space_available() && !charging_is_robot_seeking())
300
                wl_charging_send_station_available(source);
301
        wl_charging_send_battery(source);
302
}
303

    
304
/**
305
 * Called when we receive a request to charge.
306
 *
307
 * @param source the robot requesting to charge
308
 **/
309
void wl_charging_request(int source)
310
{
311
        if (charging_is_space_available() && !charging_is_robot_seeking())
312
        {
313
                charging_begin_seeking(source);
314
                wl_charging_send_request_accept(source);
315
        }
316
        else
317
                wl_charging_send_cancel(source);
318
}
319

    
320
/**
321
 * Called when we receive a request to verify our status
322
 * in the docking procedure.
323
 *
324
 * @param source the robot verifying our status
325
 **/
326
void wl_charging_verify(int source)
327
{
328
        int state = charging_get_robot_state(source);
329

    
330
        STATION_DEBUG_PRINT("Received verify request from robot ");
331
        STATION_DEBUG_PUTI(source);
332
        STATION_DEBUG_PRINT(".\n");
333

    
334
        switch (state)
335
        {
336
                case STATE_SEEKING:
337
                        wl_charging_send_seeking(source);
338
                        break;
339
                case STATE_DOCKED:
340
                        wl_charging_send_docked(source);
341
                        break;
342
                case STATE_NOT_CHARGING:
343
                        STATION_DEBUG_PRINT("Received verify from unknown robot ");
344
                        STATION_DEBUG_PUTI(source);
345
                        STATION_DEBUG_PRINT(".\n");
346
                        wl_charging_send_cancel(source);
347
                        break;
348
                default:
349
                        STATION_DEBUG_PRINT("Unknown robot state.\n");
350
                        break;
351
        }
352
}
353

    
354
/**
355
 * Called when we receive a packet cancelling our
356
 * docking procedure.
357
 *
358
 * @param source the robot cancelling the procedure
359
 **/
360
void wl_charging_cancel(int source)
361
{
362
        int state = charging_get_robot_state(source);
363

    
364
        STATION_DEBUG_PRINT("Robot ");
365
        STATION_DEBUG_PUTI(source);
366
        STATION_DEBUG_PRINT(" has cancelled charging.\n");
367

    
368
        if (state == STATE_NOT_CHARGING)
369
        {
370
                STATION_DEBUG_PRINT("Received cancel from unknown robot.\n");
371
                return;
372
        }
373

    
374
        charging_set_verify_count(source, VERIFY_DELAY);
375

    
376
        charging_cancel(source);
377
}
378

    
379
/**
380
 * Called when we receive a packet stating that the
381
 * robot is seeking us.
382
 *
383
 * @param source the robot seeking us
384
 **/
385
void wl_charging_seeking(int source)
386
{
387
        int state = charging_get_robot_state(source);
388

    
389
        STATION_DEBUG_PRINT("Robot ");
390
        STATION_DEBUG_PUTI(source);
391
        STATION_DEBUG_PRINT(" has confirmed it is seeking.\n");
392

    
393
        if (state == STATE_NOT_CHARGING)
394
        {
395
                STATION_DEBUG_PRINT("Received seeking from unknown robot.\n");
396
                wl_charging_send_cancel(source);
397
                return;
398
        }
399
        
400
        charging_set_verify_count(source, VERIFY_DELAY);
401

    
402
        if (state == STATE_DOCKED)
403
        {
404
                STATION_DEBUG_PRINT("Should not have revert from docked to seeking.\n");
405
                if (charging_is_robot_seeking())
406
                {
407
                        charging_cancel(source);
408
                        wl_charging_send_cancel(source);
409
                }
410
                else
411
                        charging_begin_seeking(source);
412
        }
413
}
414

    
415
/**
416
 * Called when we receive a packet stating that
417
 * a robot is docked with us.
418
 * 
419
 * @param source the robot that is docked with us
420
 **/
421
void wl_charging_docked(int source)
422
{
423
        int state = charging_get_robot_state(source);
424
        
425
        STATION_DEBUG_PRINT("Robot ");
426
        STATION_DEBUG_PUTI(source);
427
        STATION_DEBUG_PRINT(" has confirmed it is docked.\n");
428
        
429
        if (state == STATE_NOT_CHARGING)
430
        {
431
                STATION_DEBUG_PRINT("Received docked from unknown robot.\n");
432
                return;
433
        }
434
        
435
        charging_set_verify_count(source, VERIFY_DELAY);
436

    
437
        //seeking has ended
438
        if (state == STATE_SEEKING)
439
                charging_dock(source);
440
}
441

    
442
/**
443
 * Called when we recieve a packet with a battery reading
444
 *
445
 * @param source the robot that is sending the battery reading
446
 **/
447
void wl_charging_battery(int source, unsigned char* packet, int length)
448
{
449
        if (charging_get_robot_state(source)==STATE_NOT_CHARGING)
450
                check_battery(source, (int)packet[0]);
451
        else
452
                update_battery(source, (int)packet[0]);
453
}