Statistics
| Revision:

root / demos / hunter_prey / src / main.c @ 1828

History | View | Annotate | Download (9.7 KB)

1
/*
2
 * Hunter-Prey main.c File - Implementation of Hunter-Prey behavior which
3
 *                 uses finite state machines to manage the behavior. A top level
4
 *                 state machine controls the high level behavior switches between
5
 *                 "hunter" and "prey" and manages the wireless communication. Two
6
 *                 additional state machines control the behavior of the robot when
7
 *                 it is in "prey" mode and when it is in "hunter" mode.
8
 *
9
 * Author: John Sexton, Colony Project, CMU Robotics Club
10
 */
11

    
12
#include <dragonfly_lib.h>
13
#include <wl_basic.h>
14
#include "hunter_prey.h"
15
#include "encoders.h"
16

    
17
#define WL_CHANNEL                                         24
18

    
19
#define BACK_THRESHOLD                         -1000
20
#define TURN_DIST                                                1024
21
#define IR_DIST_THRESHOLD                150
22
#define WAIT_DELAY_MS                                2000
23

    
24
/* State Macros */
25

    
26
/* Top Level FSM States */
27
#define TOP_INIT                                                0
28
#define TOP_HUNTER_HUNT                        1
29
#define TOP_HUNTER_TAG                        2
30
#define TOP_HUNTER_PURSUE                3
31
#define TOP_PREY_AVOID                        4
32
#define TOP_HUNTER_WAIT                        5
33
#define TOP_ERROR                                                6
34

    
35
/* Hunter FSM States */
36
#define HUNTER_SPIRAL                                0
37
#define HUNTER_CHASE                                1
38

    
39
/* Prey FSM States */
40
#define PREY_START_BACK                        0
41
#define PREY_BACKING                                1
42
#define PREY_TURN                                                2
43
#define PREY_AVOID                                        3
44

    
45

    
46
/* Function prototype declarations */
47
int hunter_FSM(int, int, int);
48
int prey_FSM(int);
49

    
50
/* Variables used to receive packets */
51
unsigned char* packet_data;
52
int data_length;
53

    
54
/*  Data buffer used to send packets */
55
char send_buffer[2];
56

    
57
int main(void)
58
{
59

    
60
    /* Initialize dragonfly board */
61
    dragonfly_init(ALL_ON);
62
                xbee_init();
63
                encoders_init();
64

    
65
    /* Initialize the basic wireless library */
66
    wl_basic_init_default();
67

    
68
    /* Set the XBee channel to assigned channel */
69
    wl_set_channel(WL_CHANNEL);
70

    
71

    
72
    /* ****** CODE HERE ******* */
73

    
74
                /* Initialize state machines */
75
                int state = TOP_INIT;
76
                int hunter_state = HUNTER_SPIRAL;
77
                int prey_state = PREY_AVOID;
78

    
79
                int frontIR = 0;
80
                int maxBOM = 0;
81
                int robotID = get_robotid();
82
                int oldTime = 0, curTime = 0;
83

    
84
                while (1) {
85

    
86
                        /* Check if we've received a wireless packet */
87
                        packet_data = wl_basic_do_default(&data_length);
88

    
89
                        /* Top level state machines */
90
                        switch(state) {
91

    
92
                                case TOP_INIT:
93
                                        orbs_set_color(RED, GREEN);
94
                                        delay_ms(500);
95
                                        orbs_set_color(GREEN, RED);
96
                                        delay_ms(500);
97

    
98
                                        /* Allow user to pick the starting behavior */
99
                                        if (button1_read()) {
100
                                                state = TOP_PREY_AVOID;
101
                                                prey_state = PREY_AVOID;
102
                                        } else {
103
                                                state = TOP_HUNTER_HUNT;
104
                                                hunter_state = HUNTER_SPIRAL;
105
                                        }
106
                                        break;
107
                                case TOP_HUNTER_HUNT:
108
                                        orbs_set_color(RED, RED);
109

    
110
                                        if (packet_data && data_length == 2
111
                                                        && packet_data[0] == HUNTER_PREY_ACTION_ACK) {
112
                                                /* If we've received an ACK, we need to wait */
113
                                                state = TOP_HUNTER_WAIT;
114
                                        } else {
115
                                                /* Record some sensor readings and check if we can TAG */
116
                                                bom_refresh(BOM_ALL);
117
                                                frontIR = range_read_distance(IR2);
118
                                                maxBOM = get_max_bom();
119
                                                if (hunter_prey_tagged(maxBOM, frontIR)) {
120
                                                        state = TOP_HUNTER_TAG;
121
                                                } else {
122
                                                        /* If we haven't tagged, then enter hunter FSM */
123
                                                        hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR);
124
                                                }
125
                                        }
126
                                        break;
127
                                case TOP_HUNTER_TAG:
128
                                        orbs_set_color(RED, PURPLE);
129

    
130
                                        if (packet_data && data_length == 2
131
                                                        && packet_data[0] == HUNTER_PREY_ACTION_ACK) {
132
                                                /* If we've received an ACK, then someone beat us to the TAG and
133
                                                  * we need to wait. */
134
                                                state = TOP_HUNTER_WAIT;
135
                                        } else {
136
                                                /* Prepare and send the TAG packet */
137
                                                send_buffer[0] = HUNTER_PREY_ACTION_TAG;
138
                                                send_buffer[1] = robotID;
139
                                                wl_basic_send_global_packet(42, send_buffer, 2);
140

    
141
                                                /* Record the time so we don't spam a TAG message on the network */
142
                                                oldTime = rtc_get();
143
                                                state = TOP_HUNTER_PURSUE;
144
                                        }
145
                                        break;
146
                                case TOP_HUNTER_PURSUE:
147
                                        orbs_set_color(RED, BLUE);
148
                                        curTime = rtc_get();
149
                                        
150
                                        if (packet_data && data_length == 2
151
                                                        && packet_data[0] == HUNTER_PREY_ACTION_ACK) {
152
                                                /* Check if we've received a new wireless packet */
153

    
154
                                                if (packet_data[1] == robotID) {
155
                                                        /* We've been ACKed, so we can now become the prey */
156
                                                        state = TOP_PREY_AVOID;
157
                                                        prey_state = PREY_START_BACK;
158
                                                } else {
159
                                                        /* If we get an ACK with a different robotID, then someone beat us
160
                                                         * to the TAG, so we must wait */
161
                                                        state = TOP_HUNTER_WAIT;
162
                                                }
163

    
164
                                        } else if (curTime - oldTime > 1) {
165
                                                /* If 1 second has ellapsed, return to normal hunting state (we can
166
                                                 * TAG again now) */
167
                                                state = TOP_HUNTER_HUNT;
168
                                        } else if (oldTime > curTime) {
169
                                                /* If for some reason the timer overflows, or the wireless library
170
                                                 * (which is also using the same timer) resets the timer,
171
                                                 * reinitialize the timer so that we don't wait too long for the
172
                                                 * timer to catch back up. */
173
                                                oldTime = curTime;
174
                                        } else {
175
                                                /* If no other behavioral changes need to be made, then continue
176
                                                 * with the hunter FSM where we left off */
177
                                                bom_refresh(BOM_ALL);
178
                                                frontIR = range_read_distance(IR2);
179
                                                maxBOM = get_max_bom();
180
                                                hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR);
181
                                        }
182
                                        break;
183
                                case TOP_PREY_AVOID:
184
                                        orbs_set_color(GREEN, GREEN);
185
                                        if (packet_data && data_length == 2
186
                                                        && packet_data[0] == HUNTER_PREY_ACTION_TAG) {
187
                                                /* Check if we've received a TAG yet. If so then send an ACK back */
188

    
189
                                                send_buffer[0] = HUNTER_PREY_ACTION_ACK;
190
                                                send_buffer[1] = packet_data[1];
191
                                                wl_basic_send_global_packet(42, send_buffer, 2);
192
                                                
193
                                                state = TOP_HUNTER_WAIT;
194
                                        } else {
195
                                                /* If we haven't received a TAG yet, continue with prey FSM */
196
                                                bom_on();
197
                                                prey_state = prey_FSM(prey_state);
198
                                        }                                        
199
                                        break;
200
                                case TOP_HUNTER_WAIT:
201
                                        /* Set orb colors and wait to give the prey the 5 second head start */
202
                                        orbs_set_color(BLUE, BLUE);
203
                                        bom_off();
204
                                        motors_off();
205
                                        delay_ms(WAIT_DELAY_MS);
206
                                        state = TOP_HUNTER_HUNT;
207
                                        hunter_state = HUNTER_SPIRAL;
208
                                        break;
209
                                case TOP_ERROR:
210
                                default:
211
                                        orbs_set_color(PURPLE, PURPLE);
212
                                        state = TOP_ERROR;
213
                                        while(1);
214
                                        break;
215
                        }
216

    
217
                }
218

    
219
    /* ****** END HERE ******* */
220

    
221
    while(1);
222

    
223
    return 0;
224

    
225
}
226

    
227

    
228
/*
229
 * prey_FSM - Prey finite state machine which starts by backing away, turning,
230
 *                 and then running and avoiding obstacles.
231
 *
232
 * Arguments:
233
 *         prey_state - Current prey state.
234
 *
235
 * returns - The new state of the prey state machine.
236
 */
237

    
238
int prey_FSM(int prey_state) {
239

    
240
        /* Variable to store the front rangefinder readings */
241
        int rangeVals[3] = {0, 0, 0};
242

    
243
        switch (prey_state) {
244

    
245
                case PREY_START_BACK:
246
                        motor_l_set(BACKWARD, 255);
247
                        motor_r_set(BACKWARD, 255);
248
                        encoder_rst_dx(LEFT);
249
                        encoder_rst_dx(RIGHT);
250
                        return PREY_BACKING;
251
                        break;
252
                case PREY_BACKING:
253
                        if (encoder_get_x(LEFT) < BACK_THRESHOLD
254
                                                                                        || encoder_get_x(RIGHT) < BACK_THRESHOLD) {
255
                                motor_l_set(BACKWARD, 255);
256
                                motor_r_set(FORWARD, 255);
257
                                encoder_rst_dx(LEFT);
258
                                encoder_rst_dx(RIGHT);
259
                                return PREY_TURN;
260
                        } else {
261
                                return PREY_BACKING;
262
                        }
263
                        break;
264
                case PREY_TURN:
265
                        if (encoder_get_x(LEFT) < -TURN_DIST
266
                                                                                        || encoder_get_x(RIGHT) > TURN_DIST) {
267
                                return PREY_AVOID;                                
268
                        } else {
269
                                return PREY_TURN;
270
                        }
271
                        break;
272
                case PREY_AVOID:
273
                        rangeVals[0] = range_read_distance(IR1);
274
                        rangeVals[1] = range_read_distance(IR2);
275
                        rangeVals[2] = range_read_distance(IR3);
276

    
277
                        /* Drive away if we detect obstacles using the rangefinders */
278
                        if (rangeVals[1] > 0 && rangeVals[1] < IR_DIST_THRESHOLD) {
279
                                if (rangeVals[0] < rangeVals[2]) {
280
                                        motor_l_set(FORWARD, 255);
281
                                        motor_r_set(BACKWARD, 255);
282
                                } else {
283
                                        motor_l_set(BACKWARD, 255);
284
                                        motor_r_set(FORWARD, 255);
285
                                }
286
                                return PREY_AVOID;
287
                        } else if (rangeVals[0] > 0 && rangeVals[0] < IR_DIST_THRESHOLD) {
288
                                motor_l_set(FORWARD, 255);
289
                                motor_r_set(FORWARD, 170);
290
                                return PREY_AVOID;
291
                        } else if (rangeVals[2] > 0 && rangeVals[2] < IR_DIST_THRESHOLD) {
292
                                motor_l_set(FORWARD, 170);
293
                                motor_r_set(FORWARD, 255);
294
                                return PREY_AVOID;
295
                        } else {                        
296
                                motor_l_set(FORWARD, 255);
297
                                motor_r_set(FORWARD, 255);
298
                                return PREY_AVOID;
299
                        }
300
                        break;
301
                default:
302
                        return PREY_AVOID;
303
                        break;
304

    
305
        }
306
        
307
        return prey_state;
308

    
309
}
310

    
311

    
312
/*
313
 * hunter_FSM - Hunter finite state machine which defaults to spiraling
314
 *                 outwards until the BOM can locate the prey. Once the BOM locates
315
 *                 the prey, chase the prey as fast as possible.
316
 *
317
 * Arguments:
318
 *         hunter_state - Current hunter state.
319
 *         maxBOM - Current maximum BOM value.
320
 *         frontIR - Current front IR rangefinder reading value.
321
 *
322
 * returns - The new state of the hunter state machine.
323
 */
324

    
325
int hunter_FSM(int hunter_state, int maxBOM, int frontIR) {
326

    
327
        switch(hunter_state) {
328

    
329
                case HUNTER_SPIRAL:
330
                        if (maxBOM != -1) {
331
                                return HUNTER_CHASE;
332
                        } else {
333
                                motor_l_set(FORWARD, 170);
334
                                motor_r_set(FORWARD, 190);
335
                                return HUNTER_SPIRAL;
336
                        }
337
                        break;
338
                case HUNTER_CHASE:
339
                                                
340
                        if (maxBOM == -1) {
341
                                return HUNTER_CHASE;
342
                        } else if (maxBOM == 4) {
343
                                motor_l_set(FORWARD, 255);
344
                                motor_r_set(FORWARD, 255);
345
                                return HUNTER_CHASE;
346
                        } else if (maxBOM == 3) {
347
                                motor_l_set(FORWARD, 255);
348
                                motor_r_set(FORWARD, 240);
349
                                return HUNTER_CHASE;
350
                        } else if (maxBOM == 5) {
351
                                motor_l_set(FORWARD, 240);
352
                                motor_r_set(FORWARD, 255);
353
                                return HUNTER_CHASE;
354
                        } else if (maxBOM < 3) {
355
                                motor_l_set(FORWARD, 255);
356
                                motor_r_set(FORWARD, 170);
357
                                return HUNTER_CHASE;
358
                        } else if (maxBOM > 5 && maxBOM <= 8) {
359
                                motor_l_set(FORWARD, 170);
360
                                motor_r_set(FORWARD, 255);
361
                                return HUNTER_CHASE;
362
                        } else if (maxBOM > 8 && maxBOM < 12) {
363
                                motor_l_set(BACKWARD, 255);
364
                                motor_r_set(FORWARD, 255);
365
                                return HUNTER_CHASE;
366
                        } else {
367
                                motor_l_set(FORWARD, 255);
368
                                motor_r_set(BACKWARD, 255);
369
                                return HUNTER_CHASE;
370
                        }
371
                        break;
372
                default:
373
                        return HUNTER_SPIRAL;
374
                        break;
375

    
376
        }
377

    
378
        return hunter_state;
379

    
380
}