Colony

#include <dragonfly_lib.h>

#include <encoders.h>

#define RADIUS 300

/*

        This is a PREVIOUS VERSION of circle2.  Circle2 uses Echo's more exact code, while this uses a different method which is more buggy.

        This code has been kept for the time being, in case some of the code is useful for recylcling (the control loop, for instance) - without

        reverting the entire repositiory.

        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

*/

extern void blink(int);

extern void search(void);

extern void approach(void);

extern void lastDrive(void);

int main(void)

{

        /* Initialize dragonfly board */

        dragonfly_init(ALL_ON);

        encoders_init();

        orb_set_color(GREEN);

        delay_ms(300);

        search();

        approach();

        while(1); /* END HERE */

        return 0;

}

void blink(int num)                                                // blinks the number of times specifed, with the orbs.

{                                                                                // useful for error checking.

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

        {

                orb_set_color(ORB_OFF);

                delay_ms(200);

                orb_set_color(RED);

                delay_ms(200);

        }

        orb_set_color(ORB_OFF);

}

void search()        //makes the robot face the beacon

{

        orb_set_color(YELLOW);        //show that the robot is searching

        bom_refresh(BOM_ALL);

        int top=bom_get_max();

        if ((top<4) || (top>11))        //determine which way to turn

        {

                motor_r_set(BACKWARD,200);

                motor_l_set(FORWARD,200);

        }

        else if((top>4) && (top<12))

        {

                motor_r_set(FORWARD,200);

                motor_l_set(BACKWARD,200);

        }

        while (top != 4)

        {

                bom_refresh(BOM_ALL);

                top=bom_get_max();

        }

        motors_off();

}

void approach()                                        // drives to the right distance.

{

        int dist = 0;

        int temp = 0;

        int counter = 0;

//        int correct = 0;

        while(dist != RADIUS)                //puts the robots in the given distance RADIUS

        {

                temp=0;

                counter = 0;

                while(counter<8)                // averages the first 8 legitimate (non -1) readings

                {

                        if(range_read_distance(IR2) == -1){}

                        else

                        {

                                temp += range_read_distance(IR2);

                                counter++;

                        }

                        delay_ms(20);

                }

                dist = temp/8;

                /*if (correct >2){

                        orb_set_color(RED);

                        break;

                        }        // stop within the dead zone: it knows it is in the dead zone if it hits +- 15 from RADIUS 3 times*/

                bom_refresh(BOM_ALL);

                int top = bom_get_max();

                while(top!=4)

                {

                        search();

                        bom_refresh(BOM_ALL);

                        top = bom_get_max();

                }        

                if ( dist<(RADIUS+30) && dist>(RADIUS-30)) {        // defines the dead zone +- 30 from the given radius

                        orb_set_color(RED);

                        break;

                }

                else if ((dist>RADIUS) || (dist==-1))

                {                        

                        ///delay_ms(200);

                        search();

                        motor_r_set(FORWARD,200);

                        motor_l_set(FORWARD,200);

                }

                else if ((dist<RADIUS))

                {

                        //delay_ms(200);

                        search();

                        motor_r_set(BACKWARD,200);

                        motor_l_set(BACKWARD,200);

                }

        }

        orb_set_color(BLUE);

        motors_off();

        /*usb_puti(range_read_distance(IR2));

        usb_puts("\n\r");

        motor_r_set(FORWARD,200);

        motor_l_set(FORWARD,200);

        delay_ms(300);

        usb_puti(range_read_distance(IR2));

        usb_puts("\n\r");

        delay_ms(300);

        usb_puti(range_read_distance(IR2));

        usb_puts("\n\r");

        delay_ms(300);

        usb_puti(range_read_distance(IR2));

        usb_puts("\n\r");*/

}

void lastDrive()                                        // used for the final drive in lab1 and not useful here; control loop code may be recycled.

{

        int lspeed = 190;

        int rspeed = 190;

        int firstldist = encoder_get_x(LEFT);

        int firstrdist = encoder_get_x(RIGHT);

        int ldist = firstldist;

        int rdist = firstrdist;

        while ( (ldist-firstldist<800) && (rdist-firstrdist<800) )

        {

                motor_l_set(FORWARD,lspeed);

                motor_r_set(FORWARD,rspeed);

                int lvel = encoder_get_v(LEFT);

                int rvel = encoder_get_v(RIGHT);

                usb_puts("\n\rLeft distance: ");

                usb_puti(ldist-firstldist);

                usb_puts("\n\rRight distance: ");

                usb_puti(rdist-firstrdist);

                usb_puts("\n\rLeft velocity: ");

                usb_puti(lvel);

                usb_puts("\n\rRight velocity: ");

                usb_puti(rvel);

                usb_puts("\n\r\n\r");

                if (rvel<lvel)                                        // adjust the speeds if the encoders have a mismatch.

                {

                        rspeed += 1;

                        lspeed -= 1;

                }

                else if (rvel>lvel)

                {

                        rspeed -= 1;

                        lspeed += 1;

                }

                ldist = encoder_get_x(LEFT);

                rdist = encoder_get_x(RIGHT);

                delay_ms(25);

        }

        motors_off();

        orb_set_color(GREEN);

}