Revision 1618
This code changed the way that the Edges move toward the Center bot, I stopped using Han Jongs methods and added the methods correctTurn, correctApproach is still not functional, also I changed the color skeem for the orbs, going to center is still buggy.
trunk/code/behaviors/formation_control/circle/circle.c | ||
---|---|---|
5 | 5 |
|
6 | 6 |
int EDGE = 0; |
7 | 7 |
int BEACON = 1; |
8 |
int timeout = 0; |
|
8 | 9 |
|
10 |
|
|
9 | 11 |
/* |
10 | 12 |
This program is used to make robots target a center (leader) robot using the BOM, |
11 | 13 |
then drive toward it and stop at a certain distance away. |
... | ... | |
34 | 36 |
*done*Wireless communication, initialization |
35 | 37 |
*/ |
36 | 38 |
|
39 |
|
|
40 |
|
|
37 | 41 |
void forward(int speed){ // set the motors to this forward speed. |
38 | 42 |
motor_l_set(FORWARD,speed); |
39 | 43 |
motor_r_set(FORWARD,speed); |
... | ... | |
77 | 81 |
else |
78 | 82 |
return 0; |
79 | 83 |
} |
84 |
|
|
85 |
|
|
86 |
int correctTurn(void) |
|
87 |
{ |
|
88 |
int bomNum = 0; |
|
89 |
bom_refresh(BOM_ALL); |
|
90 |
bomNum = bom_get_max(); |
|
91 |
usb_puti(bomNum); |
|
92 |
if(bomNum == 4) |
|
93 |
{ timeout = 0; |
|
94 |
motor_l_set(1, 200); |
|
95 |
motor_r_set(1, 200); |
|
96 |
return 0; |
|
97 |
} |
|
98 |
else |
|
99 |
{ |
|
100 |
if(bomNum == -1){ |
|
101 |
timeout++; |
|
102 |
if(timeout > 500000) |
|
103 |
{ |
|
104 |
motor_r_set(FORWARD, 210); |
|
105 |
motor_l_set(BACKWARD, 190); |
|
106 |
} |
|
107 |
} |
|
108 |
else if((bomNum >= 12) || (bomNum < 4)) |
|
109 |
{ |
|
110 |
motor_l_set(FORWARD, 200);timeout = 0; |
|
111 |
motor_r_set(BACKWARD, 200); |
|
112 |
} |
|
113 |
else |
|
114 |
{ |
|
115 |
motor_l_set(BACKWARD, 200);timeout = 0; |
|
116 |
motor_r_set(FORWARD, 200); |
|
117 |
} |
|
118 |
} |
|
119 |
return 1; |
|
120 |
} |
|
121 |
|
|
122 |
void correctApproach(void) |
|
123 |
{ |
|
124 |
int bomNum = 0; |
|
125 |
bom_refresh(BOM_ALL); |
|
126 |
bomNum = bom_get_max(); |
|
127 |
usb_puti(bomNum); |
|
128 |
if(bomNum == 4) |
|
129 |
{ |
|
130 |
motor_l_set(1, 200); |
|
131 |
motor_r_set(1, 200); |
|
132 |
} |
|
133 |
else |
|
134 |
{ |
|
135 |
if(bomNum == -1){} |
|
136 |
else if((bomNum >= 12) || (bomNum < 4)) |
|
137 |
{ |
|
138 |
motor_l_set(FORWARD, 200); |
|
139 |
motor_r_set(BACKWARD, 200); |
|
140 |
} |
|
141 |
else |
|
142 |
{ |
|
143 |
motor_l_set(BACKWARD, 200); |
|
144 |
motor_r_set(FORWARD, 200); |
|
145 |
} |
|
146 |
delay_ms(100); |
|
147 |
} |
|
148 |
} |
|
149 |
|
|
150 |
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
|
80 | 151 |
void turn_to_beacon(int max){ |
81 | 152 |
if (max>-1 && max<16){ |
82 | 153 |
int index = (max+12)%16; |
... | ... | |
102 | 173 |
else left(170); |
103 | 174 |
} |
104 | 175 |
} |
176 |
|
|
105 | 177 |
void orient(void){ |
106 | 178 |
int max_index = -1; |
107 | 179 |
while (max_index!=4) { |
... | ... | |
122 | 194 |
delay_ms(22); |
123 | 195 |
} |
124 | 196 |
} |
197 |
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
|
125 | 198 |
void go_straight(void){ // drives forward a hardcoded distance. May not be useful. |
126 | 199 |
forward(200); |
127 | 200 |
encoder_rst_dx(LEFT); |
... | ... | |
164 | 237 |
int main(void) |
165 | 238 |
{ |
166 | 239 |
/* Initialize dragonfly board */ |
167 |
dragonfly_init(ALL_ON); |
|
168 |
xbee_init(); |
|
169 |
encoders_init(); |
|
170 |
/* Initialize the basic wireless library */ |
|
171 |
wl_basic_init_default(); |
|
172 |
/* Set the XBee channel to your assigned channel */ |
|
240 |
dragonfly_init(ALL_ON); |
|
241 |
/* Initialize the basic wireless library */ |
|
242 |
wl_basic_init_default(); |
|
243 |
/* Set the XBee channel to your assigned channel */ /* Set the XBee channel to your assigned channel */ |
|
173 | 244 |
wl_set_channel(24); |
174 | 245 |
|
175 | 246 |
int robotid = get_robotid(); |
... | ... | |
202 | 273 |
{ |
203 | 274 |
case 0: // EDGE |
204 | 275 |
|
205 |
orb_set_color(GREEN);
|
|
276 |
bom_off();
|
|
206 | 277 |
while(1) |
207 | 278 |
{ |
208 |
orb_set_color(YELLOW);
|
|
279 |
orb1_set_color(YELLOW);orb2_set_color(CYAN);
|
|
209 | 280 |
packet_data=wl_basic_do_default(&data_length); |
210 | 281 |
if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST) |
211 | 282 |
{ |
... | ... | |
219 | 290 |
|
220 | 291 |
while(1) // wait for the center bot to count all the robots and send a "done" packet |
221 | 292 |
{ |
222 |
orb_set_color(RED);
|
|
293 |
orb_set_color(YELLOW);orb2_set_color(PURPLE);
|
|
223 | 294 |
packet_data=wl_basic_do_default(&data_length); |
224 | 295 |
wl_basic_send_global_packet(42,send_buffer,2); |
225 | 296 |
|
... | ... | |
229 | 300 |
} |
230 | 301 |
} |
231 | 302 |
|
232 |
orb_set_color(GREEN);
|
|
233 |
|
|
234 |
orient();
|
|
235 |
forward(speed);
|
|
303 |
orb_set_color(MAGENTA);
|
|
304 |
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
|
305 |
while(correctTurn()){
|
|
306 |
}
|
|
236 | 307 |
//range_init(); |
237 | 308 |
|
238 |
orb_set_color(BLUE); |
|
239 | 309 |
|
240 | 310 |
distance = get_distance(); |
241 | 311 |
while (distance>=onefoot || distance==0) |
242 | 312 |
{ |
243 | 313 |
if(distance==0) |
244 | 314 |
orb_set_color(WHITE); |
315 |
//correctApproach(); |
|
245 | 316 |
distance = get_distance(); |
246 |
orient2(); |
|
247 |
forward(speed); |
|
248 | 317 |
delay_ms(14); |
249 | 318 |
} |
319 |
|
|
250 | 320 |
stop(); |
251 |
orient(); |
|
252 |
|
|
321 |
orb_set_color(LIME); |
|
253 | 322 |
//button1_wait (); // code for lab1. |
254 | 323 |
//go_straight_onefoot(); |
255 |
stop(); |
|
256 | 324 |
break; |
257 | 325 |
|
258 | 326 |
|
... | ... | |
266 | 334 |
if(button1_click()) beacon_State=1; |
267 | 335 |
break; |
268 | 336 |
case 1: // sends a global exist packet to see how many robots there are |
337 |
orb_set_color(RED); |
|
269 | 338 |
send_buffer[0]=CIRCLE_ACTION_EXIST; |
270 | 339 |
send_buffer[1]=get_robotid(); |
271 | 340 |
wl_basic_send_global_packet(42,send_buffer,2); |
... | ... | |
273 | 342 |
break; |
274 | 343 |
case 2: // find out how many robots there are |
275 | 344 |
waitingCounter++; |
276 |
orb_set_color(ORANGE); |
|
345 |
orb1_set_color(YELLOW); |
|
346 |
orb2_set_color(BLUE); |
|
277 | 347 |
packet_data=wl_basic_do_default(&data_length); |
278 | 348 |
if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
279 | 349 |
{ |
280 |
orb_set_color(WHITE);
|
|
350 |
orb_set_color(RED);orb2_set_color(BLUE);
|
|
281 | 351 |
if(used[packet_data[1]]==0){ //only add to robots seen if you haven't gotten an ACK from this robot |
282 | 352 |
robotsReceived++; |
283 | 353 |
used[packet_data[1]]=1; |
... | ... | |
289 | 359 |
} |
290 | 360 |
break; |
291 | 361 |
case 3: |
292 |
orb_set_color(PURPLE);
|
|
362 |
orb_set_color(GREEN);
|
|
293 | 363 |
send_buffer[0]=CIRCLE_ACTION_DONE; |
294 | 364 |
wl_basic_send_global_packet(42,send_buffer,2); |
295 | 365 |
break; |
Also available in: Unified diff