Colony

#include <dragonfly_lib.h>

#include <encoders.h>

// TODO:

//        Check BOM more often and before final stop

//        Make BOM check more comprehensively

//        Use the center bot to check distances

//                Count them ("spam" method)

//                Use beacon to find relative positions

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

//        Wireless communication, initialization

void forward(int speed){

        motor_l_set(FORWARD,speed);

        motor_r_set(FORWARD,speed);

}

void left(int 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){

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

}

int get_distance(void){

        int temp,distance,kk=5;

        distance =0;

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

                temp = range_read_distance(IR2);

                if (temp == -1) {temp=0; kk--;}

                distance+= temp;

                delay_ms(3);

        }

        if (kk>0)

                return (int)(distance/kk);

        else 

                return 0;

}

void turn_to_beacon(int max){

        if (max>-1 && max<16){

                int index = (max+12)%16;

                if (index==0) { 

                        stop(); 

                }

                else if (index<8) right(170);

                else left(170);

        }

}

void turn_to_beacon2(int max){

        if (max>-1 && max<16){

                int index = (max+12)%16;

                if (index==0) { 

                }

                else if (index<8) right(170);

                else left(170);

        }

}

void orient(void){

        int max_index = -1;

        while (max_index!=4) {

                /* Refresh and make sure the table is updated */

                bom_refresh(BOM_ALL);

                max_index = bom_get_max();

                turn_to_beacon(max_index);

                delay_ms(22);

        }

}

void orient2(void){

        int max_index = -1;

        while (max_index!=4) {

                /* Refresh and make sure the table is updated */

                bom_refresh(BOM_ALL);

                max_index = bom_get_max();

                turn_to_beacon2(max_index);

                delay_ms(22);

        }

}

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

        }

}

void go_straight_onefoot(void){

        go_straight();

}

int main(void)

{

        /* Initialize dragonfly board */

        dragonfly_init(ALL_ON);

        encoders_init();

        /*******CODE HERE ***** */

        int distance=1000;

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

        orient();

        forward(speed);

        range_init();

        orb_set_color(YELLOW);

        distance = get_distance();

        while (distance>=onefoot || distance==0){

                distance = get_distance();

                orient2();

                forward(speed);

                delay_ms(14);

        }

        stop();

        orient();

        //button1_wait ();

        //go_straight_onefoot();

        stop();

        /* ****** CODE HERE ******* */

        while(1); /* END HERE */

        return 0;

}