Colony

#include <dragonfly_lib.h>

#include <encoders.h>

#define RADIUS 300

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)

{

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

{

        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)

                {

                        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)) {        // the dead zone

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

{

        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)

                {

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

}