Colony

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include <encoders.h>

#include "circle.h"

int EDGE = 0;

int BEACON = 1;

int timeout = 0;

int sending = 0;

int stop2 = 0;

struct vector slave_position;

int desired_max_bom;

int bom_max_counter;

//Last used 12,13,7(BOM)

void set_desired_max_bom(int desired_angle)

{

        if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0;

        if (desired_angle >= 11 && desired_angle < 33) desired_max_bom = 1;

        if (desired_angle >= 33 && desired_angle < 56) desired_max_bom = 2;

        if (desired_angle >= 56 && desired_angle < 78) desired_max_bom = 3;

        if (desired_angle >= 78 && desired_angle < 101) desired_max_bom = 4;

        if (desired_angle >= 101 && desired_angle < 123) desired_max_bom = 5;

        if (desired_angle >= 123 && desired_angle < 145) desired_max_bom = 6;

        if (desired_angle >= 145 && desired_angle < 167) desired_max_bom = 7;

        if (desired_angle >= 167 && desired_angle < 190) desired_max_bom = 8;

        if (desired_angle >= 190 && desired_angle < 212) desired_max_bom = 9;

        if (desired_angle >= 212 && desired_angle < 235) desired_max_bom = 10;

        if (desired_angle >= 235 && desired_angle < 257) desired_max_bom = 11;

        if (desired_angle >= 257 && desired_angle < 280) desired_max_bom = 12;

        if (desired_angle >= 280 && desired_angle < 302) desired_max_bom = 13;

        if (desired_angle >= 302 && desired_angle < 325) desired_max_bom = 14;

        if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15;

}

void switch_sending(void)

{

        if(sending)

        {

                sending = 0;

                bom_off();

        }

        else

        {

                sending = 1;

                bom_on();

        }

}

/*

Current situation:

        go to the center. ends when each edge robto send "I am here" message

I think compasses are in the club, but not on the robots?

Assume we have the compasses, and write pseudocode, maybe?

TODO:

  Center:

    1. Make a move forward method/state to make the circle begin moving as a group.

  Edge:

    2. Find a way to follow the middle robot and preserve circle structure, or follow a directional command given by the center robot.

    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.

  In general:

    More testing.  Especially with robots that are actually working.

*/

/*

        This program is used to make robots target a center (leader) robot using the BOM,

        then drive toward it and stop at a certain distance away.

        The distance will eventually be adjustable.

        With adjustment, the leader robot will be able to turn and use its standardized

        rangefinder to reposition or space the robots evenly.

        AuTHORS: Nico, Alex, Reva, Echo, Steve

*/

/*

TODO:

        Used: Bots 1, 7

        16 Performed Badly

        12 worked ok as beacon, not well as edge

                Fix orient code so the bot does not toggle back and forth when it tries to turn

                Use the center bot to check distances

Done-->        Count them ("spam" method)

                Use beacon to find relative positions

                Beacon tells them how to move to be at the right distance

                *done*Wireless communication, initialization

*/

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;

}

/*

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

*/

void correctTurn(void)

{

        orb1_set_color(BLUE);                        // BLUE and PURPLE

        left(220);

        while(1)

        {

                int bomNum = 0;                                // bomNum is the current maximum reading

                bom_refresh(BOM_ALL);

                bomNum = bom_get_max();

                usb_puti(bomNum);

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

                {

                        timeout = 0;

                        //motor_l_set(1, 200);

                        //motor_r_set(1, 200);

                        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 >= 12) || (bomNum < 4))

                        {

                                motor_l_set(FORWARD, 200);

                                motor_r_set(BACKWARD, 200);

                                timeout = 0;

                        }

                        else

                        {

                                motor_l_set(BACKWARD, 200);

                                motor_r_set(FORWARD, 200);

                                timeout = 0;

                        }

                }

        }

        return;

}

/*

        This is supposed to (essentially) do a correct turn, then move forward or something.

        Actually, we should just get rid of this.

*/

void correctApproach(void)

{

        int bomNum = 0;

        bom_refresh(BOM_ALL);

        bomNum = bom_get_max();

        usb_puti(bomNum);

        if(bomNum == 4)

        {

                motor_l_set(1, 200);

                motor_r_set(1, 200);

        }

        else

        {

                if(bomNum == -1){}

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

                {

                        motor_l_set(FORWARD, 200);

                        motor_r_set(BACKWARD, 200);

                }

                else

                {

                        motor_l_set(BACKWARD, 200);

                        motor_r_set(FORWARD, 200);

                }

                delay_ms(100);

        }

}

/*

        drives forward a hardcoded distance. May not be useful.

*/

void go_straight(void){

        forward(200);

        encoder_rst_dx(LEFT);

        encoder_rst_dx(RIGHT);

        delay_ms(100);

        int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);

        int count = 0;

        int d;

        while (count<25){                                                //count = 25 when bot6; count <12

                x_left = encoder_get_x(LEFT);

                x_right = encoder_get_x(RIGHT);

                d = x_right-x_left;

                if (d>13 || d<-13){

                        if(d<50 && d>-50){

                                d = round(1.0*d/4);

                                setforward(200+d, 200-d);

                        }

                }

                delay_ms(32);

                count++;

        }

}

/*

    BLINK the given number times

*/

void blink(int num) {

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

        {

                orb_set_color(ORB_OFF);

                delay_ms(350);

                orb_set_color(RED);

                delay_ms(200);

        }

        orb_set_color(ORB_OFF);

}

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

/*

        A double state machine.  The robot is either an "edge" or a "beacon" depending on the initial potentiometer reading.

*/

int main(void)

{

        /* Initialize dragonfly board */

            dragonfly_init(ALL_ON);

            /* Initialize the basic wireless library */

            wl_basic_init_default();

            /* Set the XBee channel to your assigned channel */        /* Set the XBee channel to your assigned channel */

        wl_set_channel(24);

        int sending_counter = 0;

        int robotid = get_robotid();

        int used[16];

        int usedPointer=0;

        for (int i=0; i<16; i++) used[i] = 0;

        char send_buffer[2];

        int data_length;

        unsigned char *packet_data=wl_basic_do_default(&data_length);

        int state = EDGE;

        int beacon_State=0;

        int edge_State=0;

        int waitingCounter=0;

        int robotsReceived=0;

        int offset = 20, time=0;

        if(wheel()<100)

        {

                state=EDGE;

        }

        else

        {

                state=BEACON;

        }

        int distance=1000;                                        // how far away to stop.

        int onefoot=250, speed=250;                                // one foot is 490 for bot 1; one foot is 200 for bot6

        while(1)

        {

                bom_refresh(BOM_ALL);

                /*

                *******TERMinology**************

                EDGE=0 other names: slave. Definition: robots on the edge of  the circle;

                BEACON=1 other name: master. Definition: robots in the center of the circle;

                *******EXPECTED MOVES **********

                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

                */

                // decide if its is center or not.

                switch(state)

                {

                        /**********

                                if  EDGE /slave robots

                        */

                        case 0:

                                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. Wait the center robot to finish counting all EDGE robots

                                        */

                                        case 0:

                                                bom_off();

                                                orb1_set_color(YELLOW);orb2_set_color(CYAN);

                                                packet_data=wl_basic_do_default(&data_length);

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

                                                {

                                                        send_buffer[0]=CIRCLE_ACTION_ACK;

                                                        send_buffer[1]=robotid;

                                                        wl_basic_send_global_packet(42,send_buffer,2);

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

                                                packet_data=wl_basic_do_default(&data_length);

                                                //wl_basic_send_global_packet(42,send_buffer,2);

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

                                                {

                                                        edge_State=2;

                                                }

                                        break;

                                        case 2:

                                                // COLOR afer DONE ---> MAGENTA

                                                orb_set_color(MAGENTA);

                                                correctTurn();                        // 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>500){

                                                                correctTurn();

                                                                time=0;

                                                        }

                                                }

                                                stop();

                                                orb_set_color(LIME);

                                                send_buffer[0]=CIRCLE_ACTION_ACK;

                                                send_buffer[1]=robotid;

                                                wl_basic_send_global_packet(42,send_buffer,2);

                                                edge_State=3;

                                        break;

                                        // wait until the beacon sends out its robot ID

/*

        // as we don't check distance to center one by one now,

        no more private communication at this point.

                                        case 3:

                                                orb_set_color(YELLOW);

                                                orb2_set_color(GREEN);

                                                packet_data=wl_basic_do_default(&data_length);

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

                                                {

                                                        bom_on();

                                                        orb_set_color(ORANGE);

                                                }

                                                if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE && packet_data[1]==robotid)

                                                {