Colony

#include <dragonfly_lib.h>

#include <encoders.h>

/*

        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: Niko, Alex, Reva, Echo

*/

/* 

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

        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; 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){                                // like the previous but no stop() call

        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){                                                // drives forward a hardcoded distance. May not be useful.

        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){                                // essentially, another name for the above.  Should be removed.

        go_straight();

}

int main(void)

{

        /* Initialize dragonfly board */

        dragonfly_init(ALL_ON);

        encoders_init();

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

        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 ();                        // code for lab1.

        //go_straight_onefoot();

        stop();

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

        while(1); /* END HERE */

        return 0;

}