root / trunk / code / behaviors / hunter_prey / brad / main.c @ 1501
History | View | Annotate | Download (4.68 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wireless.h> |
3 |
#include <wl_basic.h> |
4 |
#include "smart_run_around_fsm.h" |
5 |
#include "hunter_prey.h" |
6 |
|
7 |
#define CHAN 0xC |
8 |
#define TAG_PAUSE 2000 |
9 |
|
10 |
#define ROBOT_HUNTER 0 |
11 |
#define ROBOT_PREY 1 |
12 |
#define ROBOT_TAGGED 2 |
13 |
|
14 |
void packet_receive(char type, int source, unsigned char* packet, int length); |
15 |
|
16 |
volatile uint8_t robotState = ROBOT_HUNTER;
|
17 |
volatile uint8_t color1, color2;
|
18 |
volatile uint8_t tagAck;
|
19 |
volatile uint8_t tagger;
|
20 |
uint8_t id; |
21 |
|
22 |
void test_bom(void) { |
23 |
int i;
|
24 |
int val;
|
25 |
|
26 |
while(1) { |
27 |
bom_refresh(BOM_ALL); |
28 |
|
29 |
for(i=0; i<16; i++) { |
30 |
val = bom_get(i); |
31 |
usb_puti(val); |
32 |
usb_putc(' ');
|
33 |
} |
34 |
|
35 |
usb_puts("| ");
|
36 |
|
37 |
usb_puti(range_read_distance (IR1)); |
38 |
usb_putc(' ');
|
39 |
usb_puti(range_read_distance (IR2)); |
40 |
usb_putc(' ');
|
41 |
usb_puti(range_read_distance (IR3)); |
42 |
usb_putc(' ');
|
43 |
usb_puti(range_read_distance (IR4)); |
44 |
usb_putc(' ');
|
45 |
usb_puti(range_read_distance (IR5)); |
46 |
|
47 |
usb_putc('\n');
|
48 |
delay_ms(500);
|
49 |
} |
50 |
|
51 |
} |
52 |
|
53 |
void chase(uint8_t angle) {
|
54 |
|
55 |
int omega = angle - 4; |
56 |
|
57 |
if(angle > 16) |
58 |
return;
|
59 |
|
60 |
if(omega > 8) |
61 |
omega = omega - 16;
|
62 |
|
63 |
omega *= 255/8; |
64 |
|
65 |
/* usb_puti(omega); */
|
66 |
/* usb_putc('\n'); */
|
67 |
|
68 |
move(140, omega);
|
69 |
|
70 |
} |
71 |
|
72 |
int main(void) { |
73 |
|
74 |
int frontRange;
|
75 |
uint8_t angle; |
76 |
unsigned char packet[2]; |
77 |
|
78 |
//intialize the robot and wireless
|
79 |
dragonfly_init(ALL_ON); |
80 |
|
81 |
// test_bom();
|
82 |
|
83 |
id = get_robotid(); |
84 |
packet[1] = id;
|
85 |
usb_puts("hello, I am robot \n");
|
86 |
usb_puti(id); |
87 |
usb_putc('\n');
|
88 |
|
89 |
usb_puts("wireless.\n");
|
90 |
|
91 |
wl_basic_init(&packet_receive); |
92 |
|
93 |
usb_puts("run around.\n");
|
94 |
|
95 |
run_around_init(); |
96 |
|
97 |
usb_puts("channel\n");
|
98 |
|
99 |
//use a specific channel to avoid interfering with other tasks
|
100 |
wl_set_channel(CHAN); |
101 |
|
102 |
usb_puts("orb set\n");
|
103 |
|
104 |
orb_set_color(BLUE); |
105 |
color1 = BLUE; |
106 |
color2 = BLUE; |
107 |
|
108 |
//if button 2 is held, you are prey
|
109 |
if(button2_read()) {
|
110 |
robotState = ROBOT_PREY; |
111 |
bom_on(); |
112 |
} |
113 |
|
114 |
|
115 |
while(1) { |
116 |
|
117 |
if(robotState==ROBOT_TAGGED) {
|
118 |
usb_puts("tagged, waiting to send ACK\n");
|
119 |
//delay_ms(TAG_PAUSE);
|
120 |
move(0,0); |
121 |
usb_puts("sending ACK to ");
|
122 |
usb_puti(tagger); |
123 |
usb_putc('\n');
|
124 |
packet[0] = HUNTER_PREY_ACTION_ACK;
|
125 |
packet[1] = tagger;
|
126 |
wl_basic_send_global_packet (42, &packet, 2); |
127 |
usb_puts("sent\n");
|
128 |
robotState = ROBOT_HUNTER; //no longer prey
|
129 |
} |
130 |
|
131 |
else if(robotState == ROBOT_PREY) { |
132 |
run_around_FSM(); |
133 |
} |
134 |
|
135 |
else { //ROBOT_HUNTER |
136 |
|
137 |
bom_refresh(BOM_ALL); |
138 |
|
139 |
delay_ms(100);
|
140 |
|
141 |
frontRange = range_read_distance (IR2); |
142 |
|
143 |
/* usb_puti(frontRange); */
|
144 |
/* usb_putc(','); */
|
145 |
|
146 |
//BUG: when get_max_bom is called, the rangefinder seems to stop
|
147 |
//working
|
148 |
angle = bom_get_max(); |
149 |
/* usb_puti(angle); */
|
150 |
/* usb_putc('\n'); */
|
151 |
|
152 |
chase(angle); |
153 |
|
154 |
//debugging to determine tagging conditions
|
155 |
if(angle < 7 && angle > 1) { |
156 |
if(color1 != RED) {
|
157 |
color1 = RED; |
158 |
orb1_set_color(RED); |
159 |
} |
160 |
} |
161 |
else {
|
162 |
if(color1 != BLUE) {
|
163 |
color1 = BLUE; |
164 |
orb1_set_color(BLUE); |
165 |
} |
166 |
} |
167 |
|
168 |
|
169 |
usb_puti(frontRange); |
170 |
usb_putc('\n');
|
171 |
|
172 |
/* if(frontRange > 0 && frontRange < TAG_RANGE) { */
|
173 |
/* if(color2 != RED) { */
|
174 |
/* color2 = RED; */
|
175 |
/* orb2_set_color(RED); */
|
176 |
/* } */
|
177 |
/* } */
|
178 |
/* else { */
|
179 |
/* if(color2 != BLUE) { */
|
180 |
/* color2 = BLUE; */
|
181 |
/* orb2_set_color(BLUE); */
|
182 |
/* } */
|
183 |
/* } */
|
184 |
|
185 |
|
186 |
|
187 |
if(hunter_prey_tagged(angle, frontRange) || button1_read()) {
|
188 |
orb_set_color(YELLOW); |
189 |
color1 = YELLOW; |
190 |
color2 = YELLOW; |
191 |
usb_puts("TAG!\n");
|
192 |
tagAck = 0;
|
193 |
packet[0] = HUNTER_PREY_ACTION_TAG;
|
194 |
packet[1] = id;
|
195 |
wl_basic_send_global_packet (42, &packet, 2); |
196 |
move(0,0); |
197 |
while(!tagAck)
|
198 |
wl_do(); |
199 |
|
200 |
if(tagAck == id) { //if the ack was for us |
201 |
usb_puts("ACK!\n");
|
202 |
|
203 |
move(-230,0); //back up to give the new prey some room |
204 |
delay_ms(TAG_PAUSE/2);
|
205 |
move(0,0); |
206 |
|
207 |
robotState = ROBOT_PREY; |
208 |
bom_on(); |
209 |
} |
210 |
else {
|
211 |
usb_puts("ACK failed!\n");
|
212 |
} |
213 |
} |
214 |
} |
215 |
|
216 |
wl_do(); |
217 |
} |
218 |
|
219 |
return 0; |
220 |
} |
221 |
|
222 |
void packet_receive(char type, int source, unsigned char* packet, int length) |
223 |
{ |
224 |
|
225 |
if(type==42 && length>=2){ |
226 |
if(packet[0] == HUNTER_PREY_ACTION_TAG) { |
227 |
if(robotState == ROBOT_PREY) {
|
228 |
robotState = ROBOT_TAGGED; |
229 |
bom_off(); |
230 |
color1 = BLUE; |
231 |
color2 = BLUE; |
232 |
orb_set_color(BLUE); |
233 |
tagger = packet[1];
|
234 |
} |
235 |
else {
|
236 |
usb_puts("tagged, but I'm not it!\n");
|
237 |
} |
238 |
} |
239 |
else if(packet[0] == HUNTER_PREY_ACTION_ACK && robotState == ROBOT_HUNTER) { |
240 |
tagAck = packet[1];
|
241 |
} |
242 |
else {
|
243 |
usb_puts("got '");
|
244 |
usb_putc(packet[0]);
|
245 |
usb_puti(packet[1]);
|
246 |
usb_puts("'\n");
|
247 |
} |
248 |
} |
249 |
else {
|
250 |
usb_puts("got unkown packet! type=");
|
251 |
usb_puti(type); |
252 |
usb_puts(" length=");
|
253 |
usb_puti(length); |
254 |
usb_putc('\n');
|
255 |
} |
256 |
} |
257 |
|
258 |
#define HP_BUF_IDX_ACTION 0 |