Colony

   This program assembles a group of robots into a circle and allows the

movement within that formation.  Robots should be able to break formation and

travel as a line, readjust in the face of obstacles, and reform if conditions

are necessary.

   The program begins waiting for a button press.  When pressed, a robot assumes

the BEACON position, which means that it is the robot in the center of the

circle and therefore in charge.  It then gathers robots around it by sending

them commands.  This code is executed using two finite state machines, nested

inside one another.

   One controls the overall state of the robot (whether it is a BEACON, an EDGE,

or WAITING, for example).

   This code should be implemented so that most useful functions are built in

to the machine.  For example, the BEACON robot should be able to call methods

such as CircleUp() to gather robots around it, and Move(distance) to move the

circle group all at once.

   This Code is the property of the Carnegie Mellon Robotics Club and is being

used to test formation control in a low-cost robot colony.  Thanks to all

members of RoboClub, especially Colony president John Sexton and graduade

student representative Chris Mar.

   AUTHORS: James Carroll, Steve DeVincentis, Hanzhang (Echo) Hu, Nico Paris,

Joel Rey, Reva Street, Alex Zirbel	 						*/

   -Transform the code into a method-based state machine that uses the

procedural state machines, which are hardcoded and hard to edit, as a backup.

   -Implement a drive straight method for use in keeping the robots more

accurate as a group.

   -Fix the approach method: good robots usually work well, but bad robots often

have errors which might be avoidable with the use of error checking.

   -Make robots more robust: packages are often lost, which throws the entire

procedural nature of the program off.

   -Consider using the center bot to check distances

   -More testing is always good and necessary.					*/

   4-1-2010: BOT 7 as BEACON and BOT 1 as EDGE worked extremely well.

   4-2-2010: BOT 7 and BOT 14 worked extremely well, no matter states.  BOT 1

       	started well, but malfunctioned later.					        */

	The robot waits to be given a signal to do something.  Wireless is on, in

	motor_l_set(BACKWARD,speed);

	motor_r_set(FORWARD,speed);

}

void right(int speed){