Project

General

Profile

Statistics
| Revision:

root / trunk / code / behaviors / hunter_prey / john / main.c @ 1500

History | View | Annotate | Download (9.53 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                                         15
18

    
19
#define BACK_THRESHOLD                         -5000
20
#define TURN_DIST                                                1024
21
#define IR_DIST_THRESHOLD                200
22

    
23
/* State Macros */
24

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

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

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

    
44

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

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

    
53
/*  Data buffer used to send packets */
54
char send_buffer[1];
55

    
56
int main(void)
57
{
58

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

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

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

    
70

    
71
    /* ****** CODE HERE ******* */
72

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

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

    
83
                while (1) {
84

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

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

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

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

    
109
                                        if (packet_data && data_length == 2
110
                                                        && packet_data[0] == HUNTER_PREY_ACTION_ACK) {
111
                                                /* If we've received an ACK, we need to wait */
112
                                                state = TOP_HUNTER_WAIT;
113
                                        } else {
114
                                                /* Record some sensor readings and check if we can TAG */
115
                                                bom_refresh(BOM_ALL);
116
                                                frontIR = range_read_distance(IR2);
117
                                                maxBOM = get_max_bom();
118
                                                if (hunter_prey_tagged(maxBOM, frontIR) || button1_read()) {
119
                                                        state = TOP_HUNTER_TAG;
120
                                                } else {
121
                                                        /* If we haven't tagged, then enter hunter FSM */
122
                                                        hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR);
123
                                                }
124
                                        }
125
                                        break;
126
                                case TOP_HUNTER_TAG:
127
                                        orbs_set_color(RED, PURPLE);
128
                                        delay_ms(500);
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
                                                bom_off();
194
                                                motors_off();
195
                                                state = TOP_HUNTER_WAIT;
196
                                        } else {
197
                                                /* If we haven't received a TAG yet, continue with prey FSM */
198
                                                bom_on();
199
                                                prey_state = prey_FSM(prey_state);
200
                                        }                                        
201
                                        break;
202
                                case TOP_HUNTER_WAIT:
203
                                        /* Set orb colors and wait to give the prey the 5 second head start */
204
                                        orbs_set_color(BLUE, BLUE);
205
                                        delay_ms(5000);
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
                        return PREY_BACKING;
250
                        break;
251
                case PREY_BACKING:
252
                        if (encoder_get_x(LEFT) < BACK_THRESHOLD) {
253
                                motor_l_set(BACKWARD, 255);
254
                                motor_r_set(FORWARD, 255);
255
                                encoder_rst_dx(RIGHT);
256
                                return PREY_TURN;
257
                        } else {
258
                                return PREY_BACKING;
259
                        }
260
                        break;
261
                case PREY_TURN:
262
                        if (encoder_get_x(RIGHT) > TURN_DIST) {
263
                                return PREY_AVOID;                                
264
                        } else {
265
                                return PREY_TURN;
266
                        }
267
                        break;
268
                case PREY_AVOID:
269
                        rangeVals[0] = range_read_distance(IR1);
270
                        rangeVals[1] = range_read_distance(IR2);
271
                        rangeVals[2] = range_read_distance(IR3);
272

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

    
296
        }
297
        
298
        return prey_state;
299

    
300
}
301

    
302

    
303
/*
304
 * hunter_FSM - Hunter finite state machine which defaults to spiraling
305
 *                 outwards until the BOM can locate the prey. Once the BOM locates
306
 *                 the prey, chase the prey as fast as possible.
307
 *
308
 * Arguments:
309
 *         hunter_state - Current hunter state.
310
 *         maxBOM - Current maximum BOM value.
311
 *         frontIR - Current front IR rangefinder reading value.
312
 *
313
 * returns - The new state of the hunter state machine.
314
 */
315

    
316
int hunter_FSM(int hunter_state, int maxBOM, int frontIR) {
317

    
318
        switch(hunter_state) {
319

    
320
                case HUNTER_SPIRAL:
321
                        if (maxBOM != -1) {
322
                                return HUNTER_CHASE;
323
                        } else {
324
                                motor_l_set(FORWARD, 170);
325
                                motor_r_set(FORWARD, 190);
326
                                return HUNTER_SPIRAL;
327
                        }
328
                        break;
329
                case HUNTER_CHASE:
330
                        
331
                        /* A large look-up table to set the speed of the motors based on
332
                         * where the prey robot is. */
333
                        if (maxBOM == -1) {
334
                                return HUNTER_CHASE;
335
                        } else if (maxBOM == 4) {
336
                                motor_l_set(FORWARD, 255);
337
                                motor_r_set(FORWARD, 255);
338
                                return HUNTER_CHASE;
339
                        } else if (maxBOM == 3) {
340
                                motor_l_set(FORWARD, 255);
341
                                motor_r_set(FORWARD, 240);
342
                                return HUNTER_CHASE;
343
                        } else if (maxBOM == 5) {
344
                                motor_l_set(FORWARD, 240);
345
                                motor_r_set(FORWARD, 255);
346
                                return HUNTER_CHASE;
347
                        } else if (maxBOM < 3) {
348
                                motor_l_set(FORWARD, 255);
349
                                motor_r_set(FORWARD, 170);
350
                                return HUNTER_CHASE;
351
                        } else if (maxBOM > 5 && maxBOM <= 8) {
352
                                motor_l_set(FORWARD, 170);
353
                                motor_r_set(FORWARD, 255);
354
                                return HUNTER_CHASE;
355
                        } else if (maxBOM > 8 && maxBOM < 12) {
356
                                motor_l_set(BACKWARD, 255);
357
                                motor_r_set(FORWARD, 255);
358
                                return HUNTER_CHASE;
359
                        } else {
360
                                motor_l_set(FORWARD, 255);
361
                                motor_r_set(BACKWARD, 255);
362
                                return HUNTER_CHASE;
363
                        }
364
                        break;
365
                default:
366
                        return HUNTER_SPIRAL;
367
                        break;
368

    
369
        }
370

    
371
        return hunter_state;
372

    
373
}