Colony

/*** PROGRAM INFORMATION ***

          This program assembles a group of robots into a circle and allows them 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 the ever-present 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.                */

int END = 100;

int WAITINGSTATE = 0;                /* Define some variables to keep track of the state machine.*/

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 currentPos = 0;

int state = 0;

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();

        }

}

void forward(int speed){                        // set the motors to this forward speed.

        motor_l_set(FORWARD,speed);

        motor_r_set(FORWARD,speed);

}

void left(int speed){                                // turn left at this speed.

        motor_l_set(FORWARD,speed);

        motor_r_set(BACKWARD,speed);

}

void right(int speed){

        motor_l_set(BACKWARD,speed);

        motor_r_set(FORWARD,speed);

}

void stop(void){                        // could be set to motors_off(), or just use this as an alternative.

        motor_l_set(BACKWARD,0);        // stop() is better - motors_off() creates a slight delay to turn them back on.

        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);

}

int get_distance(void){                                // takes an averaged reading of the front rangefinder

        int temp,distance,kk=5;                        // kk sets this to 5 readings.

        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 aboutFace(int goal)

{

        int goala = goal;

        int goalb = goal + 8;                        // the inverse of the goal direction, across the BOM.

        if(goalb >= 16) { goalb -= 16; }

        int inv = 0;

        if(goala > goalb)

        {

                goala = goalb;

                goalb = goal;

                inv = 1;

        }

        orb1_set_color(BLUE);                        // BLUE and PURPLE

        left(220);

        while(1)

        {

                // bomNum is the current maximum reading

                bom_refresh(BOM_ALL);

                int bomNum = bom_get_max();

                if(bomNum == goal)                                // when it's turned the right way, stop

                {

                        timeout = 0;

                        stop();

                        break;                                // exits the while() loop to stop the method

                }

                else                                        // facing the wrong way

                {

                        if(bomNum == -1)

                        {

                                timeout++;

                                if(timeout > 5000)        // if it's been looking too long, move a little bit as it turns

                                {

                                        motor_r_set(FORWARD, 210);

                                        motor_l_set(BACKWARD, 190);

                                }

                        }

                        else if((bomNum >= goalb) || (bomNum < goala))

                        {

                                if(inv)

                                        right(200);

                                else

                                        left(200);

                                timeout = 0;

                        }

                        else

                        {

                                if(inv)

                                        left(200);

                                else

                                        right(200);

                                timeout = 0;

                        }

                }

        }

        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);

}

void order(int action)

{

        currentPos++;

        send2(CIRCLE_EXECUTE, action);

        state = 20 + action;

}

void terminate(void)

{

        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();

        int centerid = 0;                // once the EDGE gets the first signal from a center, it stores who the center is.

        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.

        for (int i=0; i<17; i++) used[i] = 0;                // initially, no robots in the group.

        int data_length;                // keeps track of the length of wireless packets received.

        unsigned char *packet_data=wl_basic_do_default(&data_length);

        int beacon_State=0;                // these variables keep track of the inner state machines in the procedural MACHINE states.

        int edge_State=0;

        int waitingCounter=0;

        int robotsReceived=0;                // an important variable that stores the size of the group.

        int offset = 20;                // offset for the approaching: how far off the rangefinders can be

        int time=0;

        int direction = 4;                // keeps track of which way robots are facing relative to the center

        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

                                                // right distance away.

        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())

                                {

                                        send2(CIRCLE_CLAIM_CENTER, robotid);         // becomes the center if button1 is clicked.

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

                                                order(DRIVE); break;

                                        case 4:

                                                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);

                                                send2(CIRCLE_ACTION_ACK,robotid);                // keep sending the packet until we get a response

                                                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;

                                        case 2:        // wait for the second, general, done packet.

                                                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);

                                                aboutFace(4);                        // turn to face the beacon

                                                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);

                                                        //correctApproach();

                                                        distance = get_distance();

                                                        delay_ms(14);

                                                        time+=14;

                                                        if(time>50)

                                                        {

                                                                aboutFace(4);

                                                                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 (DIRECTION) */

                                        case 3:

                                                aboutFace(direction);

                                        break;

                                }

                        break;

                        /* The MOVE steps for the EDGE robot */

                        case 13:

                                switch(edge_State)

                                {

                                        /* Wait for the command to move forward. */

                                        case 0:

                                                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);

                                                        edge_State = 1;

                                                }

                                        break;

                                        /* Terminal. */

                                        case 1:

                                                stop();

                                                state = 1;

                                        break;

                                }        // end the EdgeState switch

                        break;                // break the Edge state in the main switch loop

                        // 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 robots 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/position */

                                        case 0:

                                                left(170);

                                                orb1_set_color(YELLOW);

                                                orb2_set_color(WHITE);

                                                int numOk = 0;

                                                while(numOk<robotsReceived)

                                                {

                                                        packet_data=wl_basic_do_default(&data_length);

                                                        if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)

                                                        {

                                                                numOk++;

                                                        }

                                                }

                                                state = 2;

                                        break;

                                }

                        break;

                        /* The ORIENT code for the beacon */

                        case 22:

                                switch(beacon_State)

                                {

                                        /* Turns all the robots in the same direction */

                                        case 0:

                                                stop();

                                                bom_off();

                                                orb_set_color(ORANGE);

                                                // for each robot, tells them to turn their bom on, then tells them which way they should face.

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

                                                {

                                                        if(used[i] == 1)

                                                        {

                                                                send2(CIRCLE_ACTION_GOTYOU, i);

                                                                while(1)        // waits for a response so it knows the BOM is on.

                                                                {

                                                                        orb_set_color(RED);

                                                                        orb2_set_color(WHITE);

                                                                        packet_data=wl_basic_do_default(&data_length);

                                                                        if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)

                                                                        {

                                                                                orb_set_color(ORANGE);

                                                                                break;

                                                                        }

                                                                }

                                                                bom_refresh(BOM_ALL);

                                                                direction = bom_get_max();

                                                                direction += 8;

                                                                if(direction > 15) direction -= 16;

                                                                send3(CIRCLE_ACTION_GOTYOU, i, direction);

                                                                delay_ms(20);

                                                        }

                                                }

                                                beacon_State = 1;

                                        break;

                                        /* Sends a DONE packet to signify that it has read in all the robots' directions and sent packets.

                                                Edge robots should now turn to face the right direction. */

                                        case 1:

                                                send2(CIRCLE_ACTION_DONE,robotid);

                                                bom_on();

                                                state = 2;

                                        break;

                                }

                        break;

                        /* The DRIVE code for the beacon */

                        case 23:

                                switch(beacon_State)

                                {

                                        /* Tells the robots to move forward and moves itself. */

                                        case 0:

                                                orb_set_color(YELLOW);

                                                delay_ms(5000);

                                                // format: type of ack, speed divided by 10, time in seconds.

                                                send3(CIRCLE_ACTION_FORWARD,20,2);

                                                orb_set_color(BLUE);

                                                forward(200);

                                                delay_ms(2000);

                                                stop();

                                                beacon_State = 1;

                                        break;

                                        /* Terminal. */

                                        case 1:

                                                stop();

                                                state = 2;

                                        break;

                                }

                        break;

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

                }        // ends the main switch

        }                // ends the main while loop

        orb_set_color(RED);        // error, we should never break from the while loop!

        while(1); /* END HERE, just in case something happened.  This way we can see the red orb. */

}