Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (6.8 KB)

1
#include <dragonfly_lib.h>
2
#include <wl_basic.h>
3
#include "hunter_prey.h"
4
#include "encoders.h"
5

    
6
#define WL_CHANNEL                                         15
7

    
8
#define BACK_THRESHOLD                         -5000
9
#define TURN_DIST                                                1024
10
#define IR_DIST_THRESHOLD                200
11

    
12
/* State Macros */
13

    
14
/* Top Level FSM */
15
#define TOP_INIT                                                0
16
#define TOP_HUNTER_HUNT                        1
17
#define TOP_HUNTER_TAG                        2
18
#define TOP_HUNTER_PURSUE                3
19
#define TOP_PREY_AVOID                        4
20
#define TOP_HUNTER_WAIT                        5
21
#define TOP_ERROR                                                6
22

    
23
/* Hunter FSM */
24
#define HUNTER_SPIRAL                                0
25
#define HUNTER_CHASE                                1
26

    
27
/* Prey FSM */
28
#define PREY_START_BACK                        0
29
#define PREY_BACKING                                1
30
#define PREY_TURN                                                2
31
#define PREY_AVOID                                        3
32

    
33

    
34
/* Function prototype declarations */
35
int hunter_FSM(int, int, int);
36
int prey_FSM(int);
37

    
38
/* Variables used to receive packets */
39
unsigned char* packet_data;
40
int data_length;
41

    
42
/*  Data buffer used to send packets */
43
char send_buffer[1];
44

    
45
int main(void)
46
{
47

    
48
    /* Initialize dragonfly board */
49
    dragonfly_init(ALL_ON);
50
                xbee_init();
51
                encoders_init();
52

    
53
    /* Initialize the basic wireless library */
54
    wl_basic_init_default();
55

    
56
    /* Set the XBee channel to assigned channel */
57
    wl_set_channel(WL_CHANNEL);
58

    
59

    
60
    /* ****** CODE HERE ******* */
61

    
62
                /* Initialize state machines */
63
                int state = TOP_INIT;
64
                int hunter_state = HUNTER_SPIRAL;
65
                int prey_state = PREY_AVOID;
66

    
67
                int frontIR = 0;
68
                int maxBOM = 0;
69
                int robotID = get_robotid();
70
                int oldTime = 0, curTime = 0;
71

    
72
                while (1) {
73

    
74
                        packet_data = wl_basic_do_default(&data_length);
75

    
76
                        switch(state) {
77

    
78
                                case TOP_INIT:
79
                                        orbs_set_color(RED, GREEN);
80
                                        delay_ms(500);
81
                                        orbs_set_color(GREEN, RED);
82
                                        delay_ms(500);
83

    
84
                                        if (button1_read()) {
85
                                                state = TOP_PREY_AVOID;
86
                                                prey_state = PREY_AVOID;
87
                                        } else {
88
                                                state = TOP_HUNTER_HUNT;
89
                                                hunter_state = HUNTER_SPIRAL;
90
                                        }
91
                                        break;
92
                                case TOP_HUNTER_HUNT:
93
                                        orbs_set_color(RED, RED);
94

    
95
                                        if (packet_data && data_length == 2
96
                                                        && packet_data[0] == HUNTER_PREY_ACTION_ACK) {
97
                                                state = TOP_HUNTER_WAIT;
98
                                        } else {
99
                                                bom_refresh(BOM_ALL);
100
                                                frontIR = range_read_distance(IR2);
101
                                                maxBOM = get_max_bom();
102
                                                if (hunter_prey_tagged(maxBOM, frontIR) || button1_read()) {
103
                                                        state = TOP_HUNTER_TAG;
104
                                                } else {
105
                                                        hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR);
106
                                                }
107
                                        }
108
                                        break;
109
                                case TOP_HUNTER_TAG:
110
                                        orbs_set_color(RED, PURPLE);
111
                                        delay_ms(500);
112

    
113
                                        if (packet_data && data_length == 2
114
                                                        && packet_data[0] == HUNTER_PREY_ACTION_ACK) {
115
                                                state = TOP_HUNTER_WAIT;
116
                                        } else {
117
                                                send_buffer[0] = HUNTER_PREY_ACTION_TAG;
118
                                                send_buffer[1] = robotID;
119
                                                wl_basic_send_global_packet(42, send_buffer, 2);
120

    
121
                                                oldTime = rtc_get();
122
                                                state = TOP_HUNTER_PURSUE;
123
                                        }
124
                                        break;
125
                                case TOP_HUNTER_PURSUE:
126
                                        orbs_set_color(RED, BLUE);
127
                                        curTime = rtc_get();
128
                                        
129
                                        if (packet_data && data_length == 2
130
                                                        && packet_data[0] == HUNTER_PREY_ACTION_ACK) {
131

    
132
                                                if (packet_data[1] == robotID) {
133
                                                        state = TOP_PREY_AVOID;
134
                                                        prey_state = PREY_START_BACK;
135
                                                } else {
136
                                                        state = TOP_HUNTER_WAIT;
137
                                                }
138

    
139
                                        } else if (curTime - oldTime > 1) {
140
                                                state = TOP_HUNTER_HUNT;
141
                                        } else if (oldTime > curTime) {                // If overflow or was reset, restart
142
                                                oldTime = curTime;
143
                                        } else {
144
                                                bom_refresh(BOM_ALL);
145
                                                frontIR = range_read_distance(IR2);
146
                                                maxBOM = get_max_bom();
147
                                                hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR);
148
                                        }
149
                                        break;
150
                                case TOP_PREY_AVOID:
151
                                        orbs_set_color(GREEN, GREEN);
152
                                        if (packet_data && data_length == 2
153
                                                        && packet_data[0] == HUNTER_PREY_ACTION_TAG) {
154

    
155
                                                send_buffer[0] = HUNTER_PREY_ACTION_ACK;
156
                                                send_buffer[1] = packet_data[1];
157
                                                wl_basic_send_global_packet(42, send_buffer, 2);
158
                                                
159
                                                bom_off();
160
                                                motors_off();
161
                                                state = TOP_HUNTER_WAIT;
162
                                        } else {
163
                                                bom_on();
164
                                                prey_state = prey_FSM(prey_state);
165
                                        }                                        
166
                                        break;
167
                                case TOP_HUNTER_WAIT:
168
                                        orbs_set_color(BLUE, BLUE);
169
                                        delay_ms(5000);
170
                                        state = TOP_HUNTER_HUNT;
171
                                        hunter_state = HUNTER_SPIRAL;
172
                                        break;
173
                                case TOP_ERROR:
174
                                default:
175
                                        orbs_set_color(PURPLE, PURPLE);
176
                                        state = TOP_ERROR;
177
                                        while(1);
178
                                        break;
179
                        }
180

    
181
                }
182

    
183
    /* ****** END HERE ******* */
184

    
185
    while(1);
186

    
187
    return 0;
188

    
189
}
190

    
191
int prey_FSM(int prey_state) {
192

    
193
        int rangeVals[3] = {0, 0, 0};
194
        
195
        switch (prey_state) {
196

    
197
                case PREY_START_BACK:
198
                        motor_l_set(BACKWARD, 255);
199
                        motor_r_set(BACKWARD, 255);
200
                        encoder_rst_dx(LEFT);
201
                        return PREY_BACKING;
202
                        break;
203
                case PREY_BACKING:
204
                        if (encoder_get_x(LEFT) < BACK_THRESHOLD) {
205
                                motor_l_set(BACKWARD, 255);
206
                                motor_r_set(FORWARD, 255);
207
                                encoder_rst_dx(RIGHT);
208
                                return PREY_TURN;
209
                        } else {
210
                                return PREY_BACKING;
211
                        }
212
                        break;
213
                case PREY_TURN:
214
                        if (encoder_get_x(RIGHT) > TURN_DIST) {
215
                                return PREY_AVOID;                                
216
                        } else {
217
                                return PREY_TURN;
218
                        }
219
                        break;
220
                case PREY_AVOID:
221
                        rangeVals[0] = range_read_distance(IR1);
222
                        rangeVals[1] = range_read_distance(IR2);
223
                        rangeVals[2] = range_read_distance(IR3);
224

    
225
                        if (rangeVals[1] > 0 && rangeVals[1] < IR_DIST_THRESHOLD) {
226
                                motor_l_set(FORWARD, 255);
227
                                motor_r_set(BACKWARD, 255);
228
                                return PREY_AVOID;
229
                        } else if (rangeVals[0] > 0 && rangeVals[0] < IR_DIST_THRESHOLD) {
230
                                motor_l_set(FORWARD, 255);
231
                                motor_r_set(FORWARD, 170);
232
                                return PREY_AVOID;
233
                        } else if (rangeVals[2] > 0 && rangeVals[2] < IR_DIST_THRESHOLD) {
234
                                motor_l_set(FORWARD, 170);
235
                                motor_r_set(FORWARD, 255);
236
                                return PREY_AVOID;
237
                        } else {                        
238
                                motor_l_set(FORWARD, 255);
239
                                motor_r_set(FORWARD, 255);
240
                                return PREY_AVOID;
241
                        }
242
                        break;
243
                default:
244
                        return PREY_AVOID;
245
                        break;
246

    
247
        }
248
        
249
        return prey_state;
250

    
251
}
252

    
253
int hunter_FSM(int hunter_state, int maxBOM, int frontIR) {
254

    
255
        switch(hunter_state) {
256

    
257
                case HUNTER_SPIRAL:
258
                        if (maxBOM != -1) {
259
                                return HUNTER_CHASE;
260
                        } else {
261
                                motor_l_set(FORWARD, 170);
262
                                motor_r_set(FORWARD, 190);
263
                                return HUNTER_SPIRAL;
264
                        }
265
                        break;
266
                case HUNTER_CHASE:
267
                        if (maxBOM == -1) {
268
                                return HUNTER_CHASE;
269
                        } else if (maxBOM == 4) {
270
                                motor_l_set(FORWARD, 255);
271
                                motor_r_set(FORWARD, 255);
272
                                return HUNTER_CHASE;
273
                        } else if (maxBOM == 3) {
274
                                motor_l_set(FORWARD, 255);
275
                                motor_r_set(FORWARD, 240);
276
                                return HUNTER_CHASE;
277
                        } else if (maxBOM == 5) {
278
                                motor_l_set(FORWARD, 240);
279
                                motor_r_set(FORWARD, 255);
280
                                return HUNTER_CHASE;
281
                        } else if (maxBOM < 3) {
282
                                motor_l_set(FORWARD, 255);
283
                                motor_r_set(FORWARD, 170);
284
                                return HUNTER_CHASE;
285
                        } else if (maxBOM > 5 && maxBOM <= 8) {
286
                                motor_l_set(FORWARD, 170);
287
                                motor_r_set(FORWARD, 255);
288
                                return HUNTER_CHASE;
289
                        } else if (maxBOM > 8 && maxBOM < 12) {
290
                                motor_l_set(BACKWARD, 255);
291
                                motor_r_set(FORWARD, 255);
292
                                return HUNTER_CHASE;
293
                        } else {
294
                                motor_l_set(FORWARD, 255);
295
                                motor_r_set(BACKWARD, 255);
296
                                return HUNTER_CHASE;
297
                        }
298
                        break;
299
                default:
300
                        return HUNTER_SPIRAL;
301
                        break;
302

    
303
        }
304

    
305
        return hunter_state;
306

    
307
}