root / trunk / code / behaviors / hunter_prey / james / main.c @ 1847
History | View | Annotate | Download (4.95 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wl_basic.h> |
3 |
#include "hunter_prey.h" |
4 |
|
5 |
#define WL_CHANNEL 15 |
6 |
#define HUNTER 3 |
7 |
#define PREY 1 |
8 |
#define PREHUNTER 2 |
9 |
#define PREPREY 0 |
10 |
#define WAITING 4 |
11 |
|
12 |
/* Variables used to receive packets */
|
13 |
unsigned char* packet_data; |
14 |
int data_length;
|
15 |
|
16 |
/* Data buffer used to send packets */
|
17 |
char send_buffer[2]; |
18 |
|
19 |
int main(void) |
20 |
{ |
21 |
|
22 |
/* Initialize dragonfly board */
|
23 |
dragonfly_init(ALL_ON); |
24 |
/* Initialize the XBEE */
|
25 |
xbee_init(); |
26 |
/* Initialize the basic wireless library */
|
27 |
wl_basic_init_default(); |
28 |
/* Set the XBee channel to your assigned channel */
|
29 |
wl_set_channel(WL_CHANNEL); |
30 |
/* Initialize Orbs*/
|
31 |
orb_init(); |
32 |
/* Initialize Clock*/
|
33 |
rtc_init(SIXTEENTH_SECOND, NULL);
|
34 |
/* Initialize motors*/
|
35 |
motors_init(); |
36 |
|
37 |
int state=PREPREY, time, max_bom_reading, front_rangefinder_reading, angle, d1=80, d2=80, d3=80, d4=80, d5=80, speed=0; |
38 |
unsigned char id=get_robotid(), tag; |
39 |
|
40 |
if(wheel() < 100) |
41 |
state=HUNTER; |
42 |
|
43 |
while(1){ |
44 |
switch(state)
|
45 |
{ |
46 |
case PREPREY:
|
47 |
{ |
48 |
orb1_set_color(YELLOW); |
49 |
bom_on(); |
50 |
state=PREY; |
51 |
motor_r_set(BACKWARD,200);
|
52 |
motor_l_set(BACKWARD,200);
|
53 |
delay_ms(2000);
|
54 |
while(wl_basic_do_default(&data_length));
|
55 |
orb1_set_color(GREEN); |
56 |
rtc_reset(); |
57 |
break;
|
58 |
} |
59 |
case PREY:
|
60 |
{ |
61 |
int t1 = 0; |
62 |
int t2 = 0; |
63 |
int timechecked = 0; |
64 |
int beginning = 1; |
65 |
orb1_set_color(GREEN); |
66 |
orb2_set_color(GREEN); |
67 |
|
68 |
while(1) |
69 |
{ |
70 |
int front = range_read_distance(IR2);
|
71 |
packet_data = wl_basic_do_default(&data_length); |
72 |
if(packet_data != 0) |
73 |
{ |
74 |
if(data_length == 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG) |
75 |
{ |
76 |
send_buffer[0] = HUNTER_PREY_ACTION_ACK;
|
77 |
send_buffer[1] = packet_data[1]; |
78 |
wl_basic_send_global_packet(42, send_buffer, 2); |
79 |
state=PREHUNTER; |
80 |
motor_r_set(0,0);motor_l_set(0,0); |
81 |
break;
|
82 |
} |
83 |
} |
84 |
int side1 = range_read_distance(IR1);
|
85 |
int side2 = range_read_distance(IR3);
|
86 |
t2 = rtc_get(); |
87 |
if(front < 200 && front > 50 && !(timechecked == 1)) |
88 |
{ |
89 |
orb2_set_color(RED); |
90 |
timechecked = 1;
|
91 |
t1 = rtc_get(); |
92 |
motor_l_set(1, 255); |
93 |
motor_r_set(0, 0); |
94 |
} |
95 |
else if(side1 < 300 && side1 > 0 && timechecked != 1) |
96 |
{ |
97 |
motor_r_set(0,0); |
98 |
} |
99 |
else if(side2 < 300 && side2 > 0 && timechecked != 1) |
100 |
{ |
101 |
motor_l_set(0,0); |
102 |
} |
103 |
else
|
104 |
{ |
105 |
if(t2 - t1 > 2 || beginning == 1) |
106 |
{ |
107 |
orb2_set_color(BLUE); |
108 |
timechecked = 0;
|
109 |
motor_l_set(1, 255); |
110 |
motor_r_set(1, 255); |
111 |
beginning = 0;
|
112 |
} |
113 |
} |
114 |
} |
115 |
break;
|
116 |
} |
117 |
case PREHUNTER: /*new hunter mode (5 second pause)*/ |
118 |
{ |
119 |
orb1_set_color(BLUE); |
120 |
orb2_set_color(BLUE); |
121 |
motors_off(); |
122 |
state=HUNTER; |
123 |
rtc_reset(); |
124 |
bom_off(); |
125 |
delay_ms(3000);
|
126 |
while(wl_basic_do_default(&data_length));
|
127 |
orb1_set_color(RED); |
128 |
break;
|
129 |
} |
130 |
case HUNTER:
|
131 |
{ |
132 |
orb1_set_color(RED); |
133 |
orb2_set_color(RED); |
134 |
/*HUNT*/
|
135 |
angle=0; speed=FULL_SPD;
|
136 |
if(max_bom_reading==4){ |
137 |
motor_l_set(FORWARD, 255);
|
138 |
motor_r_set(FORWARD, 255);
|
139 |
} |
140 |
if(max_bom_reading>=1 && max_bom_reading<=3){ |
141 |
motor_l_set(FORWARD, 255);
|
142 |
motor_r_set(FORWARD, 190);
|
143 |
} |
144 |
if(max_bom_reading>=5 && max_bom_reading<=7){ |
145 |
motor_l_set(FORWARD, 190);
|
146 |
motor_r_set(FORWARD, 255);
|
147 |
} |
148 |
if((max_bom_reading>=8 && max_bom_reading<=11) || max_bom_reading<0){ |
149 |
motor_l_set(BACKWARD, 170);
|
150 |
motor_r_set(FORWARD, 255);
|
151 |
orb2_set_color(RED); |
152 |
} |
153 |
if((max_bom_reading>=12 && max_bom_reading<=15) || max_bom_reading==0){ |
154 |
motor_l_set(FORWARD, 255);
|
155 |
motor_r_set(BACKWARD, 170);
|
156 |
} |
157 |
/*Tagging*/
|
158 |
packet_data = wl_basic_do_default(&data_length); |
159 |
if(packet_data != 0 && packet_data[0]==HUNTER_PREY_ACTION_ACK && packet_data[1]!=id){ |
160 |
state=PREHUNTER; |
161 |
} |
162 |
bom_refresh(BOM_ALL); |
163 |
front_rangefinder_reading = -1;
|
164 |
while(front_rangefinder_reading == -1){ |
165 |
front_rangefinder_reading = range_read_distance(IR2); |
166 |
} |
167 |
delay_ms(10);
|
168 |
max_bom_reading = bom_get_max(); |
169 |
tag = hunter_prey_tagged(max_bom_reading, front_rangefinder_reading); |
170 |
time = rtc_get(); |
171 |
if(button2_read()==1 || (tag && time>16)){ |
172 |
send_buffer[0]=HUNTER_PREY_ACTION_TAG;
|
173 |
send_buffer[1]=id;
|
174 |
wl_basic_send_global_packet( 42, send_buffer, 2 ); |
175 |
rtc_reset(); |
176 |
state=WAITING; |
177 |
orb1_set_color(PURPLE); |
178 |
} |
179 |
break;
|
180 |
} |
181 |
case WAITING: /*Waiting for ack after sending tag*/ |
182 |
{ |
183 |
time = rtc_get(); |
184 |
packet_data = wl_basic_do_default(&data_length); |
185 |
if(packet_data != 0 && packet_data[0]==HUNTER_PREY_ACTION_ACK && packet_data[1]==id){ |
186 |
state=PREPREY; |
187 |
rtc_reset(); |
188 |
} |
189 |
if(packet_data != 0 && packet_data[0]==HUNTER_PREY_ACTION_ACK && packet_data[1]!=id){ |
190 |
state=PREHUNTER; |
191 |
} |
192 |
if(time>16){ |
193 |
state=HUNTER; |
194 |
orb1_set_color(RED); |
195 |
} |
196 |
break;
|
197 |
} |
198 |
} |
199 |
} |
200 |
|
201 |
while(1); |
202 |
|
203 |
return 0; |
204 |
|
205 |
} |
206 |
|