Colony

/*** PROGRAM INFORMATION ***

   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

         */

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include <encoders.h>

#include "circle.h"

/*** TODO: ***

   -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.

*/

/*** BOT LOG ***

   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.

        */

/*** TERMINOLOGY ***

  WAITINGSTATE:

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

in case the robot is called on to turn into an EDGE.  The color should be LIME

or YELLOW-GREEN.

  BEACON_CONTROL:

   The code that executes commands when a robot is turned to BEACON mode.  This

code may run predefined methods for simplicity.  One goal is to make these

methods change the robot turn to to BEACON_MACHINE mode for a while, and then

return to the CONTROL code where they left off.

  EDGE_CONTROL:

   Like BEACON_CONTROL, executes whatever orders are required of the robot as an

EDGE.

  BEACON_MACHINE:

   A hardcoded list of functions which the robot is capable of running through.

Consists of a finite state machine, where the robot executes a set of commands

in a procedural manner and then returns to wherever it was in the control code.

  EDGE_MACHINE:

   Like the BEACON_MACHINE, but contains the same sort of procedural information

for EDGE robots.

  END:

   A terminal state of the machine, where the robot just sits and waits.  The

color should be GREEN and WHITE.

  TYPES OF WIRELESS PACKETS:

   CIRCLE_ACTION_EXIST 'E'

   CIRCLE_ACTION_POSITION 'P'

   CIRCLE_ACTION_ACK 'A'

         A general acknowledgement package.

   CIRCLE_ACTION_DONE 'D'

         Used by robots to tell when they have finished their action.

   CIRCLE_ACTION_GOTYOU 'G'

         Used by the BEACON to tell a robot when it has been checked off.

         At this point, the EDGE has been recognized.  Used for times when

         all EDGE robots have to communicate to the center via the spam method.

   CIRCLE_ACTION_FORWARD 'F'

         The BEACON tells the rest of the robots to move forward.

   CIRCLE_CLAIM_CENTER 'C'

         Sent out by a robot when it takes over as BEACON.

         */

/* Define some variables to keep track of the state machine.*/

int END = 100;

int WAITINGSTATE = 0;

int EDGE_CONTROL = 1;

int BEACON_CONTROL = 2;

int EDGE_MACHINE = 3;

int BEACON_MACHINE = 4;

int COUNT = 0;

int CIRCLEUP = 1;

int ORIENT = 2;

int DRIVE = 3;

int TURNL = 4;

int TURNR = 5;

int currentPos = 0;

int state = 0;

// keep track of the speed and duration of group movements.

int speed = 20;

int duration = 2;

int timeout = 0;

int sending = 0;

int stop2 = 0;

struct vector slave_position;

int desired_max_bom;

int bom_max_counter;

void switch_sending(void)

{

        if(sending)

        {

                sending = 0;

                bom_off();

        }

        else

        {

                sending = 1;

                bom_on();

        }

}

// set the motors to this forward speed.

void forward(int speed)

{

        motor_l_set(FORWARD,speed);

        motor_r_set(FORWARD,speed);

}

// turn left at this speed.

void left(int speed)

{

        motor_l_set(BACKWARD,speed);

        motor_r_set(FORWARD,speed);

}

void right(int speed)

{

        motor_l_set(FORWARD,speed);

        motor_r_set(BACKWARD,speed);

}

// stop() is better than motors_off(), which creates a slight delay when

// reactivating the motors.  Stop() is faster.

void stop(void)

{

        motor_l_set(BACKWARD,0);

        motor_r_set(FORWARD,0);

}

void setforward(int spd1, int spd2)

{

        motor_l_set(FORWARD,spd1);

        motor_r_set(FORWARD,spd2);

}

void backward(int speed)

{

        motor_l_set(BACKWARD, speed);

        motor_r_set(BACKWARD, speed);

}

// takes an averaged reading of the front rangefinder

int get_distance(void)

{

        // kk sets this to 5 readings.

        int temp,distance,kk=5;

        distance =0;

        for (int i=0; i<kk; i++)

        {

                temp = range_read_distance(IR2);

                if (temp == -1)

                {

                        //temp=0;

                        i--;

                }

                else

                        distance+= temp;

                delay_ms(3);

        }

        if (kk>0)

                return (int)(distance/kk);

        else

                return 0;

}

/* Sends a global packet with two arguments */

void send2(char arg0, char arg1)

{

        char send_buffer[2];

        send_buffer[0]=arg0;

        send_buffer[1]=arg1;

        wl_basic_send_global_packet(42,send_buffer,2);

}

/* Sends a global packet with three arguments */

void send3(char arg0, char arg1, char arg2)

{

        char send_buffer[3];

        send_buffer[0]=arg0;

        send_buffer[1]=arg1;

        send_buffer[2]=arg2;

        wl_basic_send_global_packet(42,send_buffer,3);

}

/*

   Orients the robot so that it is facing the beacon (or the broadcasting BOM).

*/

void faceFront(void)

{

        int counter = 0;

        int currentDir = 0;

        left(200);

        int bomNum = -1;

        orb1_set_color(BLUE);

        while(bomNum != 4)

        {

                if(counter >= 5)

                {

                        forward(200);

                        delay_ms(750);

                        counter = 0;

                }

                bom_refresh(BOM_ALL);

                bomNum = bom_get_max();

                if(bomNum == -1)

                {

                        //ignore

                }

                else if((bomNum < 4) || (bomNum >= 12))

                {

                        right(200);

                        if(currentDir == 0)

                                counter++;

                        currentDir = 1;

                }

                else

                {

                        left(200);

                        if(currentDir == 1)

                                counter++;

                        currentDir = 0;

                }

        }

        stop();

        return;

}

/*

   Turns the robot slowly to the right until it reaches the BOM reading goal.

   More stable code than what was implemented ealier, with smart turning,

   but slower.

*/

void aboutFace(int goal)

{

        int bomNum = -1;

        int speed = 170;        // speed with which to turn

        orb1_set_color(BLUE);        // BLUE and PURPLE

        while(bomNum != goal)

        {

                // bomNum is the current maximum reading

                bom_refresh(BOM_ALL);

                bomNum = bom_get_max();

                right(speed);

        }

        stop();

        return;

}

/*

   BLINK the given number times

*/

void blink(int num)

{

        for(int i = 0; i<num; i++)

        {

                orb_set_color(ORB_OFF);

                delay_ms(150);

                orb_set_color(RED);

                delay_ms(50);

        }

        orb_set_color(ORB_OFF);

}

/*

   BLINK slowly the given number times

*/

void slowblink(int num)

{

        for(int i = 0; i<num; i++)

        {

                orb_set_color(ORB_OFF);

                delay_ms(300);

                orb_set_color(RED);

                delay_ms(200);

        }

        orb_set_color(ORB_OFF);

}

/*

   A method for the higher-level code for the BEACON.  The beacon can make

   any of the preprogrammed commands, and this code sends the packet and

   transitions the robots correctly.

*/

void order(int action)

{

        currentPos++;

        send2(CIRCLE_EXECUTE, action);

        state = 20 + action;

}

/*

   A method for the higher-level code for the BEACON.  The beacond sends

   not only the command, but also the speed and duration for which the

   (movement) command is to be executed.

*/

void orderMove(int action, int newSpeed, int newDuration)

{

        currentPos++;

        speed = newSpeed;

        duration = newDuration;

        send2(CIRCLE_EXECUTE, action);

        state = 20 + action;

}

/*

   Turns off the motors, sends an EXECUTE packet, and blinks green and white

   forever.

*/

void terminate(void)

{

        motors_off();

        send2(CIRCLE_EXECUTE, 100);

        orb_set_color(GREEN);

        orb2_set_color(WHITE);

        while(1) ;

}

//******************************************************************************

//******************************************************************************

//******************************************************************************

/*

   A state machine with five states.  The robot starts out in WAITINGSTATE mode,

from which it recieves a signal of some sort and moves to a different state.

*/

int main(void)

{

        /* Initialize dragonfly board */

            dragonfly_init(ALL_ON);

            /* Initialize the basic wireless library */

            wl_basic_init_default();

            /* Set the XBee channel to 24 - must be standard among robots */

        wl_set_channel(24);

        int robotid = get_robotid();

        // once the EDGE gets the first signal from a center, it stores who the         // center is.

        int centerid = 0;

        // 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.

        int used[17];

        int numOk;

        // initially, no robots in the group.

        for (int i=0; i<17; i++)

                used[i] = 0;

        // keeps track of the length of wireless packets received.

        int data_length;

        unsigned char *packet_data=wl_basic_do_default(&data_length);

        // these variables keep track of the inner state machines in the

        //  procedural MACHINE states.

        int beacon_State=0;

        int edge_State=0;

        int waitingCounter=0;

        // an important variable that stores the size of the group.

        int robotsReceived=0;

        // offset for the approaching: how far off the rangefinders can be

        int offset = 20;

        int time=0;

        // keeps track of which way robots are facing relative to the center

        int direction = 4;

        // how far away the robot is.  Initialized to a large value to ensure

        // that the robot doesn't think it is already the right distance away.

        int distance=1000;

        int onefoot = 250;        // how far away to stop.

        while(1)

        {

                bom_refresh(BOM_ALL);

        /***EXPECTED MOVES***

           (OUT OF DATE.  Will be updated once changes have been made.)

        The designed movement:

         1. one center robot, several edge robots are on;

         2. center robots: button 1 is pressed;

         3. center robots: send global package telling edges that he exists;

         4. EDGE robots response with ACK.

         5. EDGE robots wait for center robots to finish counting (DONE package)

         6. EDGE robtos approach the center robtot and stop at the "onefoot"

                 distance, send message to the center

                */

                /*

                  This is the MAIN SWITCH LOOP, which governs the overall

                  status of the robot.

                 */

                switch(state)

                {

                /*

                The WAITINGSTATE.  This state constantly checks for wireless

                packets,

                and updates its state as soon as it receives a signal.

                */

                case 0:

                        orb_set_color(YELLOW);

                        packet_data=wl_basic_do_default(&data_length);

                        if(packet_data != 0 && data_length>=2

                                && packet_data[0]==CIRCLE_CLAIM_CENTER)

                        {

                                centerid = packet_data[1];

                                state = 1;

                        }

                        if(button1_read())

                        {

                                // becomes the center if button1 is clicked.

                                send2(CIRCLE_CLAIM_CENTER, robotid);

                                state = 2;

                        }

                break;

//******************************************************************************

//******************************************************************************

                /*

                The CONTROL for the EDGE state.  This sets a certain procedure

                to follow, in the form of simple

                commands, for a robot to follow if it is set to an EDGE.

                */

                case 1:

                        orb_set_color(CYAN);

                        orb1_set_color(YELLOW);

                        int command = -1;

                        packet_data=wl_basic_do_default(&data_length);

                        if(packet_data != 0 && data_length>=2 &&

                           packet_data[0]==CIRCLE_EXECUTE)

                        {

                                command = packet_data[1];

                        }

                        if(command != -1)

                        {

                                edge_State = 0;

                                switch(command)

                                {

                                case 0:

                                        state = 10; break;

                                case 1:

                                        state = 11; break;

                                case 2:

                                        state = 12; break;

                                case 3:

                                        state = 13; break;

                                case 4:

                                        state = 14; break;

                                case 5:

                                        state = 15; break;

                                case 100:

                                        terminate(); break;

                                }

                        }

                break;

//******************************************************************************

//******************************************************************************

                /*

                The CONTROL for the BEACON state.  This sets a certain procedure

                to follow, in the form of simple commands, for a robot to follow

                if it is set to a BEACON.

                */

                case 2:

                        orb_set_color(PURPLE);

                        beacon_State = 0;

                        switch(currentPos)

                        {

                        case 0:

                                order(COUNT);        break;

                        case 1:

                                order(CIRCLEUP); break;

                        case 2:

                                order(ORIENT); break;

                        case 3:

                                orderMove(DRIVE,20,2); break;

                        case 4:

                                order(CIRCLEUP); break;

                        case 5:

                                order(ORIENT); break;

                        case 6:

                                orderMove(TURNR,18,1); break;

                        case 7:

                                orderMove(DRIVE,20,2); break;

                        case 8:

                                order(CIRCLEUP); break;

                        case 9:

                                order(ORIENT); break;

                        case 10:

                                terminate(); break;

                        }

                break;

//******************************************************************************

//******************************************************************************

        /* The following states are MACHINE states for the EDGE robot. */

                /*

                        EDGE on COUNT

                */

                case 10:

                        switch(edge_State)

                        {

                                /*

                                0. EDGE robots are on.

                                1. They are waiting for EXIST pacakage from the

                                        Center robots

                                2. After they receive the package, they send ACK

                                        package to center.

                                3. Done for now: display green.

                                */

                        case 0:

                                bom_off();

                                orb1_set_color(YELLOW);

                                orb2_set_color(BLUE);

                                packet_data=wl_basic_do_default(&data_length);

                                if(packet_data != 0 && data_length>=2 &&

                                   packet_data[0]==CIRCLE_ACTION_EXIST)

                                {

                                        centerid = packet_data[1];

                                        send2(CIRCLE_ACTION_ACK,robotid);

                                        edge_State=1;

                                }

                        break;

                        /*

                                1. Wait for DONE package

                                2. The counting process is DONE

                        */

                        case 1:

                                orb_set_color(YELLOW);

                                orb2_set_color(PURPLE);

                                // keep sending the packet until we get a

                                // response

                                send2(CIRCLE_ACTION_ACK,robotid);

                                packet_data=wl_basic_do_default(&data_length);

                                if(packet_data != 0 && data_length>=2 &&

                                   packet_data[0]==CIRCLE_ACTION_GOTYOU &&

                                   packet_data[1] == robotid)

                                {

                                        edge_State=2;

                                }

                        break;

                        // wait for the second, general, done packet.

                        case 2:

                                orb_set_color(YELLOW);

                                packet_data=wl_basic_do_default(&data_length);

                                if(packet_data != 0 && data_length>=2 &&

                                   packet_data[0]==CIRCLE_ACTION_DONE &&

                                   packet_data[1] == centerid)

                                {

                                        state = 1;

                                }

                        break;

                        }

                break;

                /* The CIRCLEUP command for EDGE */

                case 11:

                        switch(edge_State)

                        {

                        case 0:

                                // COLOR afer DONE ---> MAGENTA

                                orb_set_color(MAGENTA);

                                // turn to face the beacon

                                faceFront();

                                forward(175);

                                //range_init();

                                distance = get_distance();

                                time=0;

                                while ((distance-offset)>=onefoot ||

                                   distance==0 || (distance+offset)<onefoot)

                                {

                                        if(distance==0)

                                                orb_set_color(WHITE);

                                        else if(distance-offset>=onefoot)

                                                forward(175);

                                        else

                                                backward(175);

                                        distance = get_distance();

                                        delay_ms(14);

                                        time+=14;

                                        if(time>30)

                                        {

                                                faceFront();

                                                time=0;

                                        }

                                }

                                stop();

                                orb_set_color(GREEN);

                                send2(CIRCLE_ACTION_ACK, robotid);

                                stop();

                                state = 1;

                        break;

                        }

                break;

                /* An ORIENT series of steps for the EDGE robot. */

                case 12:

                        switch(edge_State)

                        {

                        // waits for a packet to tell it to turn on the bom.

                        case 0:

                                packet_data=wl_basic_do_default(&data_length);

                                if(packet_data != 0 && data_length==2 &&

                                   packet_data[0]==CIRCLE_ACTION_GOTYOU &&

                                   packet_data[1] == robotid)

                                {

                                        bom_on();

                                        orb_set_color(ORANGE);

                                        send2(CIRCLE_ACTION_ACK,centerid);

                                        edge_State = 1;

                                }

                        break;

                        // waits for a packet to tell it that it has been

                        // received.

                        case 1:

                                orb2_set_color(YELLOW);

                                packet_data=wl_basic_do_default(&data_length);

                                if(packet_data != 0 && data_length==3 &&

                                   packet_data[0]==CIRCLE_ACTION_GOTYOU &&

                                   packet_data[1] == robotid)

                                {

                                        bom_off();

                                        direction = packet_data[2];

                                        orb_set_color(YELLOW);

                                        edge_State = 2;

                                }

                        break;

                        /*

                         Wait for the center bot to send a DONE packet; then

                         turn to face the right direction.

                        */

                        case 2:

                                orb_set_color(GREEN);

                                packet_data=wl_basic_do_default(&data_length);

                                if(packet_data != 0 && data_length>=2 &&

                                   packet_data[0]==CIRCLE_ACTION_DONE)

                                {

                                        orb_set_color(WHITE);

                                        orb2_set_color(CYAN);

                                        edge_State = 3;

                                }

                        break;

                        /* Turn until we reach the right direction */

                        case 3:

                                aboutFace(direction);

                                stop();

                                orb_set_color(YELLOW);

                                send2(CIRCLE_ACTION_DONE,robotid);

                                state = 1;

                        break;

                        }

                break;

                /* The DRIVE steps for the EDGE robot */

                case 13:

                        /* Wait for specifications to drive */

                        packet_data=wl_basic_do_default(&data_length);

                        if(packet_data != 0 && data_length>=3 &&

                           packet_data[0]==CIRCLE_ACTION_FORWARD)

                        {

                                orb_set_color(BLUE);

                                forward(packet_data[1]*10);

                                delay_ms(packet_data[2]*1000);

                                stop();

                                state = 1;

                        }

                break;

                /* The TURNL steps for the EDGE robot */

                case 14:

                        /* Wait for specifications for the turn. */

                        packet_data=wl_basic_do_default(&data_length);

                        if(packet_data != 0 && data_length>=3 &&

                           packet_data[0]==CIRCLE_ACTION_TURN)

                        {

                                orb_set_color(BLUE);

                                left(packet_data[1]*10);

                                delay_ms(packet_data[2]*1000);

                                stop();

                                state = 1;

                        }

                break;

                /* The TURNR steps for the EDGE robot */

                case 15:

                        /* Wait for specifications for the turn. */

                        packet_data=wl_basic_do_default(&data_length);

                        if(packet_data != 0 && data_length>=3 &&

                           packet_data[0]==CIRCLE_ACTION_TURN)

                        {

                                orb_set_color(BLUE);

                                right(packet_data[1]*10);

                                delay_ms(packet_data[2]*1000);

                                stop();

                                state = 1;

                        }

                break;

        // END for EDGE robots

//******************************************************************************

//******************************************************************************

                /*

                   The MACHINE for the BEACON state

                */

                /* the COUNT code for the BEACON */

                case 20:

                        switch(beacon_State)

                        {

                        /* 0. center  robots on wait for pressing button 1 */

                        case 0:

                                bom_on();

                                orb_set_color(BLUE);

                                robotsReceived = 0;

                                beacon_State=1;

                        break;

                        /* 1. Send EXIST package to EDGE robots */

                        case 1:

                                orb_set_color(RED);

                                send2(CIRCLE_ACTION_EXIST,robotid);

                                beacon_State=2;

                        break;

                        /* 2. Count the number of the EDGE robots

                         *******NOTE: at most  1000  times of loop ******  */

                        case 2:

                                waitingCounter++;

                                orb1_set_color(YELLOW);

                                orb2_set_color(BLUE);

                                packet_data=wl_basic_do_default(&data_length);

                                if(packet_data!=0 && data_length>=2 &&

                                   packet_data[0]==CIRCLE_ACTION_ACK)

                                {

                                        orb_set_color(RED);

                                        orb2_set_color(BLUE);

                                        // only add to list seen if you haven't

                                        // gotten an ACK from this robot

                                        if(used[packet_data[1]]==0)

                                        {

                                                robotsReceived++;

                                                used[packet_data[1]] = 1;

                                                usb_puts("Added: ");

                                                usb_puti(packet_data[1]);

                                                usb_puts("\r\n");

                                        }

                                        // NEW: sends a packet to each robot it

                                        // receives telling them to be done.

                                        send2(CIRCLE_ACTION_GOTYOU,

                                                packet_data[1]);

                                }

                                if(waitingCounter >= 300)

                                {

                                        beacon_State=3;

                                }

                        break;

                        /* COUNTing is DONE.  Sending DONE package. */

                        case 3:

                                blink(robotsReceived);

                                orb_set_color(GREEN);

                                send2(CIRCLE_ACTION_DONE, robotid);

                                state = 2;

                        break;

                        }

                break;

                /* The CIRCLEUP method for BEACON */

                case 21:

                        switch(beacon_State)

                        {

                        /* Wait for all the robots to get to right distance */