root / trunk / code / projects / hunter_prey / main.c @ 1409
History | View | Annotate | Download (3.79 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wireless.h> |
3 |
#include <wl_basic.h> |
4 |
#include "smart_run_around_fsm.h" |
5 |
|
6 |
#define CHAN 0xF |
7 |
#define TAG_TIME 10 |
8 |
#define TAG_RANGE 150 |
9 |
#define TAG_PAUSE 2000 |
10 |
|
11 |
#define ROBOT_HUNTER 0 |
12 |
#define ROBOT_PREY 1 |
13 |
#define ROBOT_TAGGED 2 |
14 |
|
15 |
void packet_receive(char type, int source, unsigned char* packet, int length); |
16 |
|
17 |
volatile uint8_t robotState = ROBOT_HUNTER;
|
18 |
volatile uint8_t color;
|
19 |
volatile uint8_t tagAck;
|
20 |
volatile uint8_t tagger;
|
21 |
uint8_t id; |
22 |
|
23 |
void test_bom() {
|
24 |
int i;
|
25 |
int val;
|
26 |
|
27 |
while(1) { |
28 |
bom_refresh(BOM_ALL); |
29 |
|
30 |
for(i=0; i<16; i++) { |
31 |
val = bom_get(i); |
32 |
usb_puti(val); |
33 |
usb_putc(' ');
|
34 |
} |
35 |
|
36 |
usb_putc('\n');
|
37 |
delay_ms(500);
|
38 |
} |
39 |
|
40 |
} |
41 |
|
42 |
int main(void) { |
43 |
|
44 |
int frontRange;
|
45 |
uint8_t angle; |
46 |
uint8_t i; |
47 |
int onTarget = 0; |
48 |
char packet[2]; |
49 |
uint8_t bom = 0;
|
50 |
|
51 |
//intialize the robot and wireless
|
52 |
dragonfly_init(ALL_ON); |
53 |
|
54 |
//test_bom();
|
55 |
|
56 |
id = get_robotid(); |
57 |
packet[1] = id;
|
58 |
usb_puts("hello, I am robot \n");
|
59 |
usb_puti(id); |
60 |
usb_putc('\n');
|
61 |
|
62 |
usb_puts("wireless.\n");
|
63 |
|
64 |
wl_basic_init(&packet_receive); |
65 |
|
66 |
usb_puts("run around.\n");
|
67 |
|
68 |
run_around_init(); |
69 |
|
70 |
usb_puts("channel\n");
|
71 |
|
72 |
//use a specific channel to avoid interfering with other tasks
|
73 |
wl_set_channel(CHAN); |
74 |
|
75 |
usb_puts("orb set\n");
|
76 |
|
77 |
orb_set_color(BLUE); |
78 |
color = BLUE; |
79 |
|
80 |
//if button 2 is held, you are prey
|
81 |
if(button2_read()) {
|
82 |
robotState = ROBOT_PREY; |
83 |
bom_on(); |
84 |
} |
85 |
|
86 |
|
87 |
while(1) { |
88 |
|
89 |
if(robotState==ROBOT_TAGGED) {
|
90 |
usb_puts("tagged, waiting to send ACK\n");
|
91 |
move(0,0); |
92 |
delay_ms(TAG_PAUSE); |
93 |
usb_puts("sending ACK to ");
|
94 |
usb_puti(tagger); |
95 |
usb_putc('\n');
|
96 |
packet[0] = 'A'; |
97 |
packet[1] = tagger;
|
98 |
wl_basic_send_global_packet (0, packet, 2); |
99 |
usb_puts("sent\n");
|
100 |
move(0,0); |
101 |
robotState = ROBOT_HUNTER; //no longer prey
|
102 |
} |
103 |
|
104 |
else if(robotState == ROBOT_PREY) { |
105 |
run_around_FSM(); |
106 |
} |
107 |
|
108 |
else { //ROBOT_HUNTER |
109 |
|
110 |
bom_refresh(BOM_ALL); |
111 |
|
112 |
frontRange = range_read_distance (IR2); |
113 |
|
114 |
usb_puti(frontRange); |
115 |
usb_putc(',');
|
116 |
|
117 |
//BUG: when get_max_bom is called, the rangefinder seems to stop
|
118 |
//working
|
119 |
angle = bom_get_max(); |
120 |
usb_puti(angle); |
121 |
usb_putc('\n');
|
122 |
|
123 |
if(angle == 4 && frontRange > 0 && frontRange < TAG_RANGE) { |
124 |
if(onTarget == 0) { |
125 |
onTarget = TAG_TIME; |
126 |
if(color != GREEN) {
|
127 |
color = GREEN; |
128 |
orb_set_color(GREEN); |
129 |
} |
130 |
usb_puts("On target!\n");
|
131 |
} |
132 |
else {
|
133 |
if(--onTarget <= 0) { |
134 |
orb_set_color(YELLOW); |
135 |
color = YELLOW; |
136 |
usb_puts("TAG!\n");
|
137 |
tagAck = 0;
|
138 |
packet[0] = 'T'; |
139 |
packet[1] = id;
|
140 |
wl_basic_send_global_packet (0, packet, 2); |
141 |
move(0,0); |
142 |
while(!tagAck)
|
143 |
wl_do(); |
144 |
|
145 |
if(tagAck == id) { //if the ack was for us |
146 |
usb_puts("ACK!\n");
|
147 |
// delay_ms(TAG_PAUSE);
|
148 |
robotState = ROBOT_PREY; |
149 |
bom_on(); |
150 |
} |
151 |
else {
|
152 |
usb_puts("ACK failed!\n");
|
153 |
} |
154 |
} |
155 |
} |
156 |
} |
157 |
else{
|
158 |
if(color != BLUE) {
|
159 |
color = BLUE; |
160 |
orb_set_color(BLUE); |
161 |
} |
162 |
|
163 |
//don't reset onTarget because the robot got too close
|
164 |
if(frontRange > 0) |
165 |
onTarget = 0;
|
166 |
} |
167 |
|
168 |
} |
169 |
|
170 |
wl_do(); |
171 |
} |
172 |
|
173 |
return 0; |
174 |
} |
175 |
|
176 |
void packet_receive(char type, int source, unsigned char* packet, int length) |
177 |
{ |
178 |
if(type==0 && length>=2){ |
179 |
if(packet[0] == 'T') { |
180 |
if(robotState == ROBOT_PREY) {
|
181 |
robotState = ROBOT_TAGGED; |
182 |
bom_off(); |
183 |
color = BLUE; |
184 |
orb_set_color(BLUE); |
185 |
tagger = packet[1];
|
186 |
} |
187 |
else {
|
188 |
usb_puts("tagged, but I'm not it!\n");
|
189 |
} |
190 |
} |
191 |
else if(packet[0] == 'A' && robotState == ROBOT_HUNTER) { |
192 |
tagAck = packet[1];
|
193 |
} |
194 |
else {
|
195 |
usb_puts("got '");
|
196 |
usb_putc(packet[0]);
|
197 |
usb_puti(packet[1]);
|
198 |
usb_puts("'\n");
|
199 |
} |
200 |
} |
201 |
else {
|
202 |
usb_puts("got unkown packet! type=");
|
203 |
usb_puti(type); |
204 |
usb_puts(" length=");
|
205 |
usb_puti(length); |
206 |
usb_putc('\n');
|
207 |
} |
208 |
} |
209 |
|