Revision 1749
Documented the circle code extensively. Starting reorganization from procedural to funtion-calling code.
circle.c | ||
---|---|---|
1 |
|
|
2 |
/*** PROGRAM INFORMATION *** |
|
3 |
|
|
4 |
This program assembles a group of robots into a circle and allows them movement |
|
5 |
within that formation. Robots should be able to break formation and travel as a |
|
6 |
line, readjust in the face of obstacles, and reform if conditions are necessary. |
|
7 |
|
|
8 |
The program begins waiting for a button press. When pressed, a robot assumes the |
|
9 |
BEACON position, which means that it is the robot in the center of the circle and |
|
10 |
therefore in charge. It then gathers robots around it by sending them commands. |
|
11 |
This code is executed using two finite state machines, nested inside one another. |
|
12 |
One controls the overall state of the robot (whether it is a BEACON, an EDGE, or |
|
13 |
WAITING, for example). |
|
14 |
|
|
15 |
This code should be implemented so that most useful functions are built in to the |
|
16 |
machine. For example, the BEACON robot should be able to call methods such as |
|
17 |
CircleUp() to gather robots around it, and Move(distance) to move the circle group |
|
18 |
all at once. |
|
19 |
|
|
20 |
This Code is the property of the Carnegie Mellon Robotics Club and is being used |
|
21 |
to test formation control in a low-cost robot colony. Thanks to all members of |
|
22 |
RoboClub, especially Colony president John Sexton and the ever-present Chris Mar. |
|
23 |
|
|
24 |
AUTHORS: James Carroll, Steve DeVincentis, Hanzhang (Echo) Hu, Nico Paris, Joel Rey, |
|
25 |
Reva Street, Alex Zirbel */ |
|
26 |
|
|
27 |
|
|
1 | 28 |
#include <dragonfly_lib.h> |
2 | 29 |
#include <wl_basic.h> |
3 | 30 |
#include <encoders.h> |
4 | 31 |
#include "circle.h" |
5 | 32 |
|
6 |
/* |
|
7 |
Current situation: |
|
8 |
go to the center. ends when each edge robto send "I am here" message |
|
33 |
/*** TODO: *** |
|
9 | 34 |
|
10 |
TODO: |
|
11 |
Center: |
|
12 |
1. Make a move forward method/state to make the circle begin moving as a group. |
|
35 |
- Transform the code into a method-based state machine that uses the procedural state |
|
36 |
machines, which are hardcoded and hard to edit, as a backup. |
|
37 |
- Implement a drive straight method for use in keeping the robots more accurate as a |
|
38 |
group. |
|
39 |
- Fix the approach method: good robots usually work well, but bad robots often have |
|
40 |
errors which might be avoidable with the use of error checking. |
|
41 |
- Make robots more robust: packages are often lost, which throws the entire procedural |
|
42 |
nature of the program off. |
|
43 |
- Consider using the center bot to check distances |
|
44 |
- More testing is always good and necessary. */ |
|
13 | 45 |
|
14 |
Edge: |
|
15 |
2. Find a way to follow the middle robot and preserve circle structure, or follow a directional command given by the center robot. |
|
16 |
3. Fix the error case with wireless - if the center robot does not receive the first exist packet, the robot keeps sending it and will cause error later. |
|
46 |
/*** BOT LOG *** |
|
17 | 47 |
|
18 |
In general:
|
|
19 |
More testing. Especially with robots that are actually working.
|
|
20 |
*/ |
|
48 |
4-1-2010: BOT 7 as BEACON and BOT 1 as EDGE worked extremely well.
|
|
49 |
4-2-2010: BOT 7 and BOT 14 worked extremely well, no matter states. BOT 1 started
|
|
50 |
well, but malfunctioned later. */
|
|
21 | 51 |
|
52 |
/*** TERMINOLOGY *** |
|
22 | 53 |
|
23 |
/* |
|
24 |
This program is used to make robots target a center (leader) robot using the BOM, |
|
25 |
then drive toward it and stop at a certain distance away. |
|
26 |
|
|
27 |
The distance will eventually be adjustable. |
|
54 |
WAITINGSTATE: |
|
55 |
The robot waits to be given a signal to do something. Wireless is on, in |
|
56 |
case the robot is called on to turn into an EDGE. The color should be LIME |
|
57 |
or YELLOW-GREEN. |
|
28 | 58 |
|
29 |
With adjustment, the leader robot will be able to turn and use its standardized
|
|
30 |
rangefinder to reposition or space the robots evenly.
|
|
31 |
|
|
32 |
AuTHORS: Nico, Alex, Reva, Echo, Steve, Joel
|
|
33 |
*/
|
|
59 |
BEACON_CONTROL:
|
|
60 |
The code that executes commands when a robot is turned to BEACON mode. This
|
|
61 |
code may run predefined methods for simplicity. One goal is to make these |
|
62 |
methods change the robot turn to to BEACON_MACHINE mode for a while, and then
|
|
63 |
return to the CONTROL code where they left off.
|
|
34 | 64 |
|
65 |
EDGE_CONTROL: |
|
66 |
Like BEACON_CONTROL, executes whatever orders are required of the robot as an |
|
67 |
EDGE. |
|
35 | 68 |
|
36 |
/* |
|
37 |
TODO: |
|
38 |
Used: Bots 1, 7 |
|
39 |
4-1-2010: BOT 7 as BEACON and BOT 1 as EDGE worked extremely well. |
|
40 |
16 Performed Badly |
|
41 |
12 worked ok as beacon, not well as edge |
|
42 |
|
|
43 |
Fix orient code so the bot does not toggle back and forth when it tries to turn |
|
44 |
|
|
45 |
Use the center bot to check distances |
|
46 |
Done--> Count them ("spam" method) |
|
47 |
Use beacon to find relative positions |
|
48 |
Beacon tells them how to move to be at the right distance |
|
49 |
*done*Wireless communication, initialization |
|
50 |
*/ |
|
69 |
BEACON_MACHINE: |
|
70 |
A hardcoded list of functions which the robot is capable of running through. |
|
71 |
Consists of a finite state machine, where the robot executes a set of commands |
|
72 |
in a procedural manner and then returns to wherever it was in the control code. |
|
51 | 73 |
|
52 |
int EDGE = 0; |
|
53 |
int BEACON = 1; |
|
74 |
EDGE_MACHINE: |
|
75 |
Like the BEACON_MACHINE, but contains the same sort of procedural information |
|
76 |
for EDGE robots. |
|
77 |
|
|
78 |
END: |
|
79 |
A terminal state of the machine, where the robot just sits and waits. The |
|
80 |
color should be GREEN and WHITE. |
|
81 |
|
|
82 |
|
|
83 |
TYPES OF WIRELESS PACKETS: |
|
84 |
CIRCLE_ACTION_EXIST 'E' |
|
85 |
CIRCLE_ACTION_POSITION 'P' |
|
86 |
CIRCLE_ACTION_ACK 'A' |
|
87 |
A general acknowledgement package. |
|
88 |
CIRCLE_ACTION_DONE 'D' |
|
89 |
Used by robots to tell when they have finished their action. |
|
90 |
CIRCLE_ACTION_GOTYOU 'G' |
|
91 |
Used by the BEACON to tell a robot when it has been checked off. |
|
92 |
At this point, the EDGE has been recognized. Used for times when |
|
93 |
all EDGE robots have to communicate to the center via the spam method. |
|
94 |
CIRCLE_ACTION_FORWARD 'F' |
|
95 |
The BEACON tells the rest of the robots to move forward. |
|
96 |
CIRCLE_CLAIM_CENTER 'C' |
|
97 |
Sent out by a robot when it takes over as BEACON. */ |
|
98 |
|
|
99 |
|
|
100 |
|
|
101 |
int END = 100; |
|
102 |
int WAITINGSTATE = 0; /* Define some variables to keep track of the state machine.*/ |
|
103 |
int EDGE_CONTROL = 1; |
|
104 |
int BEACON_CONTROL = 2; |
|
105 |
int EDGE_MACHINE = 3; |
|
106 |
int BEACON_MACHINE = 4; |
|
107 |
|
|
108 |
|
|
54 | 109 |
int timeout = 0; |
55 | 110 |
int sending = 0; |
56 | 111 |
int stop2 = 0; |
... | ... | |
85 | 140 |
motor_l_set(BACKWARD,speed); |
86 | 141 |
motor_r_set(FORWARD,speed); |
87 | 142 |
} |
88 |
void stop(void){ // could be set to motors_off(), or just use this as an alternative.
|
|
89 |
motor_l_set(BACKWARD,0); // stop() is better - motors_off() creates a slight delay to turn them back on.
|
|
143 |
void stop(void){ // could be set to motors_off(), or just use this as an alternative. |
|
144 |
motor_l_set(BACKWARD,0); // stop() is better - motors_off() creates a slight delay to turn them back on. |
|
90 | 145 |
motor_r_set(FORWARD,0); |
91 | 146 |
} |
92 | 147 |
void setforward(int spd1, int spd2){ |
... | ... | |
133 | 188 |
send_buffer[0]=arg0; |
134 | 189 |
send_buffer[1]=arg1; |
135 | 190 |
send_buffer[2]=arg2; |
136 |
wl_basic_send_global_packet(42,send_buffer,2);
|
|
191 |
wl_basic_send_global_packet(42,send_buffer,3);
|
|
137 | 192 |
} |
138 | 193 |
|
139 | 194 |
/* |
... | ... | |
221 | 276 |
|
222 | 277 |
|
223 | 278 |
|
224 |
|
|
225 |
|
|
226 | 279 |
//***************************************************************************************************************************************************************************************** |
280 |
//***************************************************************************************************************************************************************************************** |
|
281 |
//***************************************************************************************************************************************************************************************** |
|
227 | 282 |
|
228 | 283 |
|
229 |
|
|
230 |
|
|
231 |
|
|
232 |
|
|
233 | 284 |
/* |
234 |
A double state machine. The robot is either an "edge" or a "beacon" depending on the initial potentiometer reading. |
|
285 |
A state machine with five states. The robot starts out in WAITINGSTATE mode, from which |
|
286 |
it recieves a signal of some sort and moves to a different state. |
|
235 | 287 |
*/ |
236 | 288 |
int main(void) |
237 | 289 |
{ |
... | ... | |
239 | 291 |
dragonfly_init(ALL_ON); |
240 | 292 |
/* Initialize the basic wireless library */ |
241 | 293 |
wl_basic_init_default(); |
242 |
/* Set the XBee channel to your assigned channel */ /* Set the XBee channel to your assigned channel */
|
|
294 |
/* Set the XBee channel to 24 - must be standard among robots */
|
|
243 | 295 |
wl_set_channel(24); |
244 | 296 |
|
245 | 297 |
int robotid = get_robotid(); |
246 |
int centerid = 0; |
|
247 |
int used[17]; |
|
248 |
for (int i=0; i<17; i++) used[i] = 0; |
|
249 |
char send_buffer[2]; |
|
250 |
int data_length; |
|
298 |
int centerid = 0; // once the EDGE gets the first signal from a center, it stores who the center is.
|
|
299 |
int used[17]; // stores a list of bots which are in the group by storing a "1" in the array if the robot of that index is in the group.
|
|
300 |
for (int i=0; i<17; i++) used[i] = 0; // initially, no robots in the group.
|
|
301 |
|
|
302 |
int data_length; // keeps track of the length of wireless packets received.
|
|
251 | 303 |
unsigned char *packet_data=wl_basic_do_default(&data_length); |
252 | 304 |
|
253 |
|
|
254 |
int state = EDGE; |
|
255 |
int beacon_State=0; |
|
305 |
int state = WAITINGSTATE; |
|
306 |
int beacon_State=0; // these variables keep track of the inner state machines in the procedural MACHINE states. |
|
256 | 307 |
int edge_State=0; |
308 |
|
|
257 | 309 |
int waitingCounter=0; |
258 |
int robotsReceived=0; |
|
259 |
int offset = 20; |
|
310 |
int robotsReceived=0; // an important variable that stores the size of the group.
|
|
311 |
int offset = 20; // offset for the approaching: how far off the rangefinders can be
|
|
260 | 312 |
int time=0; |
261 |
int direction = 4; // keeps track of which way robots are facing relative to the center |
|
313 |
int direction = 4; // keeps track of which way robots are facing relative to the center |
|
314 |
int distance=1000; // how far away the robot is. Initialized to a large value to ensure that the robot doesn't think it is already the |
|
315 |
// right distance away. |
|
316 |
int onefoot = 250; // how far away to stop. |
|
262 | 317 |
|
263 |
if(wheel()>100) |
|
264 |
{ |
|
265 |
state=BEACON; |
|
266 |
} |
|
267 |
|
|
268 |
int distance=1000; // how far away to stop. |
|
269 |
int onefoot=250, speed=250; // one foot is 490 for bot 1; one foot is 200 for bot6 |
|
270 |
|
|
271 | 318 |
while(1) |
272 | 319 |
{ |
273 | 320 |
bom_refresh(BOM_ALL); |
321 |
|
|
322 |
/* |
|
323 |
*******EXPECTED MOVES ********** (OUT OF DATE. Will be updated once changes have been made.) |
|
324 |
The designed movement: |
|
325 |
1. one center robot, several edge robots are on; |
|
326 |
2. center robots: button 1 is pressed; |
|
327 |
3. center robots: send global package telling edges that he exists; |
|
328 |
4. EDGE robots response with ACK. |
|
329 |
5. EDGE robots wait for center robots to finish counting (DONE package) |
|
330 |
6. EDGE robtos approach the center robtot and stop at the "onefoot" distance, send message to the center |
|
331 |
*/ |
|
274 | 332 |
|
275 |
/* |
|
276 |
*******TERMinology************** |
|
277 |
EDGE=0 other names: slave. Definition: robots on the edge of the circle; |
|
278 |
BEACON=1 other name: master. Definition: robots in the center of the circle; |
|
279 | 333 |
|
280 |
|
|
281 |
*******EXPECTED MOVES ********** |
|
282 |
The designed movement: |
|
283 |
1. one center robot, several edge robots are on; |
|
284 |
2. center robots: button 1 is pressed; |
|
285 |
3. center robots: send global package telling edges that he exists; |
|
286 |
4. EDGE robots response with ACK. |
|
287 |
5. EDGE robots wait for center robots to finish counting (DONE package) |
|
288 |
6. EDGE robtos approach the center robtot and stop at the "onefoot" distance, send message to the center |
|
289 |
*/ |
|
290 |
|
|
291 |
|
|
292 |
// decide if its is center or not. This is the main switch loop which governs the entire state of the robot. |
|
334 |
/* This is the MAIN SWITCH LOOP, which governs the overall status of the robot. */ |
|
293 | 335 |
switch(state) |
294 | 336 |
{ |
295 |
/********** |
|
296 |
if EDGE /slave robots |
|
337 |
|
|
338 |
|
|
339 |
/* |
|
340 |
The WAITINGSTATE. This state constantly checks for wireless packets, |
|
341 |
and updates its state as soon as it receives a signal. |
|
297 | 342 |
*/ |
298 |
case 0:
|
|
343 |
case 0: |
|
299 | 344 |
|
345 |
packet_data=wl_basic_do_default(&data_length); |
|
346 |
if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_CLAIM_CENTER) |
|
347 |
{ |
|
348 |
centerid = packet_data[1]; |
|
349 |
|
|
350 |
send2(CIRCLE_ACTION_ACK,robotid); |
|
351 |
|
|
352 |
state = EDGE_CONTROL; |
|
353 |
} |
|
354 |
|
|
355 |
if(button1_read()) |
|
356 |
{ |
|
357 |
send2(CIRCLE_CLAIM_CENTER, robotid); // becomes the center if button1 is clicked. |
|
358 |
} |
|
359 |
|
|
360 |
break; |
|
361 |
|
|
362 |
|
|
363 |
|
|
364 |
//*********************************************************************************************************************************************************************************** |
|
365 |
|
|
366 |
|
|
367 |
|
|
368 |
/* |
|
369 |
The CONTROL for the EDGE state. This sets a certain procedure to follow, in the form of simple |
|
370 |
commands, for a robot to follow if it is set to an EDGE. |
|
371 |
*/ |
|
372 |
|
|
373 |
case 1: |
|
374 |
|
|
375 |
break; |
|
376 |
|
|
377 |
|
|
378 |
|
|
379 |
//*********************************************************************************************************************************************************************************** |
|
380 |
|
|
381 |
|
|
382 |
|
|
383 |
/* |
|
384 |
The CONTROL for the BEACON state. This sets a certain procedure to follow, in the form of simple |
|
385 |
commands, for a robot to follow if it is set to a BEACON. |
|
386 |
*/ |
|
387 |
case 2: |
|
388 |
|
|
389 |
break; |
|
390 |
|
|
391 |
|
|
392 |
|
|
393 |
//*********************************************************************************************************************************************************************************** |
|
394 |
|
|
395 |
|
|
396 |
/* |
|
397 |
The MACHINE for the EDGE state. |
|
398 |
*/ |
|
399 |
case 3: |
|
400 |
|
|
300 | 401 |
switch(edge_State) |
301 | 402 |
{ |
302 | 403 |
/* |
... | ... | |
369 | 470 |
distance = get_distance(); |
370 | 471 |
delay_ms(14); |
371 | 472 |
time+=14; |
372 |
if(time>500)
|
|
473 |
if(time>50) |
|
373 | 474 |
{ |
374 | 475 |
correctTurn(); |
375 | 476 |
time=0; |
... | ... | |
401 | 502 |
case 5: |
402 | 503 |
orb2_set_color(YELLOW); |
403 | 504 |
packet_data=wl_basic_do_default(&data_length); |
404 |
if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_GOTYOU && packet_data[1] == robotid)
|
|
505 |
if(packet_data != 0 && data_length==3 && packet_data[0]==CIRCLE_ACTION_GOTYOU && packet_data[1] == robotid)
|
|
405 | 506 |
{ |
406 | 507 |
bom_off(); |
407 | 508 |
direction = packet_data[2]; |
... | ... | |
410 | 511 |
} |
411 | 512 |
break; |
412 | 513 |
|
413 |
/* Blinks the direction it should turn. */
|
|
514 |
/* Wait for the center bot to send a DONE packet; then turn to face the right direction. */
|
|
414 | 515 |
case 6: |
415 |
slowblink(direction); |
|
416 |
edge_State = 7; |
|
516 |
orb_set_color(GREEN); |
|
517 |
packet_data=wl_basic_do_default(&data_length); |
|
518 |
if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE) |
|
519 |
{ |
|
520 |
orb_set_color(WHITE); |
|
521 |
orb2_set_color(CYAN); |
|
522 |
edge_State = 7; |
|
523 |
} |
|
417 | 524 |
break; |
418 | 525 |
|
526 |
/* Turn until we reach the right direction (DIRECTION) */ |
|
419 | 527 |
case 7: |
528 |
left(170); |
|
529 |
bom_refresh(BOM_ALL); |
|
530 |
if(bom_get_max() == direction) |
|
531 |
{ |
|
532 |
stop(); |
|
533 |
orb_set_color(YELLOW); |
|
534 |
edge_State = 8; |
|
535 |
} |
|
536 |
break; |
|
537 |
|
|
538 |
/* Wait for the command to move forward. */ |
|
539 |
case 8: |
|
540 |
packet_data=wl_basic_do_default(&data_length); |
|
541 |
if(packet_data != 0 && data_length>=3 && packet_data[0]==CIRCLE_ACTION_FORWARD) |
|
542 |
{ |
|
543 |
orb_set_color(BLUE); |
|
544 |
forward(200); |
|
545 |
delay_ms(2000); |
|
546 |
edge_State = END; |
|
547 |
} |
|
548 |
break; |
|
549 |
|
|
550 |
/* Terminal. */ |
|
551 |
case 100: |
|
552 |
stop(); |
|
420 | 553 |
orb_set_color(GREEN); |
554 |
orb2_set_color(WHITE); |
|
555 |
while(1); |
|
421 | 556 |
break; |
422 | 557 |
|
423 | 558 |
|
... | ... | |
432 | 567 |
|
433 | 568 |
|
434 | 569 |
|
435 |
|
|
436 |
//***************************************************************************************************************************************************************************************** |
|
437 |
//***************************************************************************************************************************************************************************************** |
|
438 |
//***************************************************************************************************************************************************************************************** |
|
570 |
//*********************************************************************************************************************************************************************************** |
|
439 | 571 |
|
440 | 572 |
|
441 | 573 |
|
442 | 574 |
|
443 |
|
|
444 | 575 |
/* |
445 |
The CENTER/BEACON/MASTER robot
|
|
576 |
The MACHINE for the BEACON state
|
|
446 | 577 |
*/ |
447 |
case 1:
|
|
578 |
case 4:
|
|
448 | 579 |
switch(beacon_State) |
449 | 580 |
{ |
450 | 581 |
|
... | ... | |
544 | 675 |
} |
545 | 676 |
} |
546 | 677 |
|
678 |
bom_refresh(BOM_ALL); |
|
547 | 679 |
direction = bom_get_max(); |
548 |
if(direction >= 16) direction -= 16; |
|
680 |
direction += 8; |
|
681 |
if(direction > 15) direction -= 16; |
|
549 | 682 |
send3(CIRCLE_ACTION_GOTYOU, i, direction); |
550 |
delay_ms(2000);
|
|
683 |
delay_ms(20); |
|
551 | 684 |
} |
552 | 685 |
} |
553 | 686 |
beacon_State = 6; |
554 | 687 |
break; |
555 | 688 |
|
689 |
/* Sends a DONE packet to signify that it has read in all the robots' directions and sent packets. |
|
690 |
Edge robots should now turn to face the right direction. */ |
|
691 |
case 6: |
|
692 |
send2(CIRCLE_ACTION_DONE,robotid); |
|
693 |
bom_on(); |
|
694 |
beacon_State = 7; |
|
695 |
break; |
|
696 |
|
|
697 |
/* Tells the robots to move forward and moves itself. */ |
|
698 |
case 7: |
|
699 |
orb_set_color(YELLOW); |
|
700 |
delay_ms(5000); |
|
701 |
|
|
702 |
// format: type of ack, speed, time. |
|
703 |
send3(CIRCLE_ACTION_FORWARD,200,2000); |
|
704 |
orb_set_color(BLUE); |
|
705 |
forward(200); |
|
706 |
delay_ms(2000); |
|
707 |
stop(); |
|
708 |
beacon_State = END; |
|
709 |
break; |
|
710 |
|
|
556 | 711 |
/* Terminal. */ |
557 |
case 6: |
|
712 |
case 100: |
|
713 |
stop(); |
|
558 | 714 |
orb_set_color(GREEN); |
715 |
orb2_set_color(WHITE); |
|
716 |
while(1); |
|
559 | 717 |
break; |
560 | 718 |
} |
561 | 719 |
break; |
720 |
|
|
721 |
//*********************************************************************************************************************************************************************************** |
|
562 | 722 |
|
563 |
} |
|
564 |
} |
|
723 |
} // ends the main switch
|
|
724 |
} // ends the main while loop
|
|
565 | 725 |
|
566 |
orb_set_color(RED); |
|
567 |
while(1); /* END HERE */ |
|
726 |
orb_set_color(RED); // error, we should never break from the while loop! |
|
568 | 727 |
|
569 |
//return 0;
|
|
728 |
while(1); /* END HERE, just in case something happened. This way we can see the red orb. */
|
|
570 | 729 |
} |
730 |
|
|
731 |
|
Also available in: Unified diff