Project

General

Profile

Statistics
| Revision:

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

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
void wl_charging_battery(int source, unsigned char* packet, int length);
41

    
42
// sending packets
43
void wl_charging_send_station_available(int dest);
44
void wl_charging_send_request_accept(int dest);
45
void wl_charging_send_verify(int dest);
46
void wl_charging_send_cancel(int dest);
47
void wl_charging_send_seeking(int dest);
48
void wl_charging_send_docked(int dest);
49
void wl_charging_send_battery(int dest);
50

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

    
57

    
58
//frames
59
#define STATION_AVAILABLE_FRAME 1
60
#define REQUEST_ACCEPT_FRAME 2
61
#define VERIFY_FRAME 3
62
#define CANCEL_FRAME 4
63
#define SEEKING_FRAME 5
64
#define DOCKED_FRAME 6
65
#define BATTERY_FRAME 7
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
 * Sends a packet to request for the robot's battery reading
166
 *
167
 * @param dest the robot to request
168
 **/
169
void wl_charging_send_battery(int dest)
170
{
171
        wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_BATTERY_REQUEST, NULL, 0, dest, BATTERY_FRAME);
172
}
173

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

    
180
}
181

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

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

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

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

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

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

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

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

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

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

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

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

    
376
        charging_set_verify_count(source, VERIFY_DELAY);
377

    
378
        charging_cancel(source);
379
}
380

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

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

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

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

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

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

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