Colony

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include <encoders.h>

#include "hive.h"

int timeout = 0;

int sending = 0;

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

}

void switch_sending(void)

{

    if(sending)

    {

        sending = 0;

        bom_off();

    }

    else

    {

        sending = 1;

        bom_on();

    }

}

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;

}

/* 

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

}

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

        orb_set_color(BLUE);

    encoder_rst_dx(LEFT);

    encoder_rst_dx(RIGHT);

        forward(200);

        delay_ms(3000);

    orb_set_color(RED);

    while(1); /* END HERE */

    //return 0;

}