root / trunk / code / projects / hunter_prey / main.c @ 1438
History | View | Annotate | Download (4.66 KB)
1 | 1373 | bneuman | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wireless.h> |
||
3 | 1408 | bneuman | #include <wl_basic.h> |
4 | #include "smart_run_around_fsm.h" |
||
5 | 1431 | bneuman | #include "hunter_prey.h" |
6 | 1373 | bneuman | |
7 | #define CHAN 0xF |
||
8 | 1408 | bneuman | #define TAG_PAUSE 2000 |
9 | 1373 | bneuman | |
10 | 1408 | bneuman | #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 | 1411 | bneuman | volatile uint8_t color1, color2;
|
18 | 1408 | bneuman | volatile uint8_t tagAck;
|
19 | volatile uint8_t tagger;
|
||
20 | uint8_t id; |
||
21 | |||
22 | 1431 | bneuman | void test_bom(void) { |
23 | 1409 | bneuman | 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 | 1410 | bneuman | 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 | 1409 | bneuman | usb_putc('\n');
|
48 | delay_ms(500);
|
||
49 | } |
||
50 | |||
51 | } |
||
52 | |||
53 | 1410 | bneuman | 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 | 1373 | bneuman | int main(void) { |
73 | |||
74 | int frontRange;
|
||
75 | uint8_t angle; |
||
76 | 1431 | bneuman | unsigned char packet[2]; |
77 | 1373 | bneuman | |
78 | //intialize the robot and wireless
|
||
79 | dragonfly_init(ALL_ON); |
||
80 | 1374 | bneuman | |
81 | 1410 | bneuman | // test_bom();
|
82 | 1409 | bneuman | |
83 | 1408 | bneuman | id = get_robotid(); |
84 | packet[1] = id;
|
||
85 | usb_puts("hello, I am robot \n");
|
||
86 | usb_puti(id); |
||
87 | usb_putc('\n');
|
||
88 | 1374 | bneuman | |
89 | 1408 | bneuman | usb_puts("wireless.\n");
|
90 | 1373 | bneuman | |
91 | 1408 | bneuman | 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 | 1373 | bneuman | //use a specific channel to avoid interfering with other tasks
|
100 | wl_set_channel(CHAN); |
||
101 | |||
102 | 1408 | bneuman | usb_puts("orb set\n");
|
103 | |||
104 | 1373 | bneuman | orb_set_color(BLUE); |
105 | 1411 | bneuman | color1 = BLUE; |
106 | color2 = BLUE; |
||
107 | 1373 | bneuman | |
108 | 1408 | bneuman | //if button 2 is held, you are prey
|
109 | if(button2_read()) {
|
||
110 | robotState = ROBOT_PREY; |
||
111 | 1409 | bneuman | bom_on(); |
112 | 1408 | bneuman | } |
113 | |||
114 | |||
115 | 1373 | bneuman | while(1) { |
116 | |||
117 | 1408 | bneuman | if(robotState==ROBOT_TAGGED) {
|
118 | usb_puts("tagged, waiting to send ACK\n");
|
||
119 | 1411 | bneuman | delay_ms(TAG_PAUSE); |
120 | 1408 | bneuman | move(0,0); |
121 | usb_puts("sending ACK to ");
|
||
122 | usb_puti(tagger); |
||
123 | usb_putc('\n');
|
||
124 | 1431 | bneuman | packet[0] = HUNTER_PREY_ACTION_ACK;
|
125 | 1408 | bneuman | packet[1] = tagger;
|
126 | 1431 | bneuman | wl_basic_send_global_packet (0, &packet, 2); |
127 | 1408 | bneuman | usb_puts("sent\n");
|
128 | robotState = ROBOT_HUNTER; //no longer prey
|
||
129 | } |
||
130 | 1373 | bneuman | |
131 | 1408 | bneuman | else if(robotState == ROBOT_PREY) { |
132 | run_around_FSM(); |
||
133 | } |
||
134 | 1373 | bneuman | |
135 | 1408 | bneuman | else { //ROBOT_HUNTER |
136 | 1373 | bneuman | |
137 | 1409 | bneuman | bom_refresh(BOM_ALL); |
138 | 1373 | bneuman | |
139 | 1411 | bneuman | delay_ms(100);
|
140 | |||
141 | 1408 | bneuman | frontRange = range_read_distance (IR2); |
142 | |||
143 | 1410 | bneuman | /* usb_puti(frontRange); */
|
144 | /* usb_putc(','); */
|
||
145 | 1408 | bneuman | |
146 | //BUG: when get_max_bom is called, the rangefinder seems to stop
|
||
147 | //working
|
||
148 | 1409 | bneuman | angle = bom_get_max(); |
149 | 1410 | bneuman | /* usb_puti(angle); */
|
150 | /* usb_putc('\n'); */
|
||
151 | 1408 | bneuman | |
152 | 1410 | bneuman | chase(angle); |
153 | |||
154 | 1431 | bneuman | //debugging to determine tagging conditions
|
155 | 1411 | bneuman | 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 | 1431 | bneuman | /* 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 | 1411 | bneuman | |
185 | |||
186 | |||
187 | 1431 | bneuman | if(hunter_prey_tagged(angle, frontRange)) {
|
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 (0, &packet, 2); |
||
196 | move(0,0); |
||
197 | while(!tagAck)
|
||
198 | wl_do(); |
||
199 | 1408 | bneuman | |
200 | 1431 | bneuman | if(tagAck == id) { //if the ack was for us |
201 | usb_puts("ACK!\n");
|
||
202 | 1411 | bneuman | |
203 | 1431 | bneuman | move(-230,0); //back up to give the new prey some room |
204 | delay_ms(TAG_PAUSE/2);
|
||
205 | move(0,0); |
||
206 | 1411 | bneuman | |
207 | 1431 | bneuman | robotState = ROBOT_PREY; |
208 | bom_on(); |
||
209 | 1408 | bneuman | } |
210 | 1431 | bneuman | else {
|
211 | usb_puts("ACK failed!\n");
|
||
212 | } |
||
213 | 1373 | bneuman | } |
214 | } |
||
215 | |||
216 | wl_do(); |
||
217 | } |
||
218 | 1431 | bneuman | |
219 | 1373 | bneuman | return 0; |
220 | } |
||
221 | 1408 | bneuman | |
222 | void packet_receive(char type, int source, unsigned char* packet, int length) |
||
223 | { |
||
224 | 1431 | bneuman | |
225 | 1408 | bneuman | if(type==0 && length>=2){ |
226 | 1431 | bneuman | if(packet[0] == HUNTER_PREY_ACTION_TAG) { |
227 | 1408 | bneuman | if(robotState == ROBOT_PREY) {
|
228 | robotState = ROBOT_TAGGED; |
||
229 | 1409 | bneuman | bom_off(); |
230 | 1411 | bneuman | color1 = BLUE; |
231 | color2 = BLUE; |
||
232 | 1408 | bneuman | orb_set_color(BLUE); |
233 | tagger = packet[1];
|
||
234 | } |
||
235 | else {
|
||
236 | usb_puts("tagged, but I'm not it!\n");
|
||
237 | } |
||
238 | } |
||
239 | 1431 | bneuman | else if(packet[0] == HUNTER_PREY_ACTION_ACK && robotState == ROBOT_HUNTER) { |
240 | 1408 | bneuman | 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 | 1431 | bneuman | #define HP_BUF_IDX_ACTION 0 |