Colony

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include<encoders.h>

int master = 0;

int sending = 0;

int stop = 0;

struct vector slave_position;

int desired_max_bom;

int bom_max_counter;

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

{

        if(sending)

        {

                sending = 0;

                bom_off();

        }

        else

        {

                sending = 1;

                bom_on();

        }

}

int main(void)

{

        int sending_counter = 0;

    /* Initialize dragonfly board */

    dragonfly_init(ALL_ON);

        xbee_init();

        wl_basic_init_default();

        wl_set_channel(12);

    int data_length;

        unsigned char *packet_data;

        char send_buffer[2];

        if(master) {orb1_set_color(RED); set_desired_max_bom(30);}

        else orb1_set_color(BLUE);

        while(1)

        {

                bom_refresh(BOM_ALL);

                if(master)

                {

                        if(sending_counter++>4)        // clock for switching the BOMs between the master and slave

                        {

                                switch_sending();

                                sending_counter = 0;

                                send_buffer[0] = 'a';

                                wl_basic_send_global_packet(42, send_buffer, 1);

                        }

                        if(sending)

                        {

                        }

                        else // recieving

                        {

                                int max_bom = bom_get_max();

                                        usb_puts("bom_get_max : ");

                                        usb_puti(max_bom);

                                        usb_puts("\n\r");

                                if(max_bom == desired_max_bom)

                                {

                                        if(bom_max_counter)                 // only stops the slave if two consecutive boms reading give the desired bom as the max one. Filters the noise.

                                        {

                                        send_buffer[0] = 's';

                                        wl_basic_send_global_packet(42, send_buffer, 2);

                                        }

                                bom_max_counter =1;

                                }

                                else bom_max_counter = 0;

                        }

                }

                else // slave

                {

                        if(packet_data[0]=='s') stop=1;

                        if(packet_data[0]=='a') switch_sending();

                        if(sending)

                        {

                        }

                        else // recieving

                        {

                                if(stop)

                                {

                                motor_l_set(FORWARD,0);

                                motor_r_set(FORWARD,0);

                                orb1_set_color(GREEN);

                                }

                                else

                                {

                                        int max_bom = bom_get_max();

                                        /*usb_puts("bom_get_max : ");

                                        usb_puti(max_bom);

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

                                   if(max_bom == 8)

                                   {        

                                        orb2_set_color(BLUE);

                                        motor_r_set(FORWARD,180);

                                        motor_l_set(FORWARD,180);

                                   }

                                   else if(max_bom == 7 || max_bom == 6 || max_bom == 5)

                                   {

                                        motor_l_set(FORWARD,180);

                                        motor_r_set(FORWARD,0);

                                        }

                                        else if(max_bom == -1);

                                   else 

                                   {

                                        orb2_set_color(GREEN);

                                        motor_l_set(FORWARD,0);

                                        motor_r_set(FORWARD,180);

                                        }

                                }

                        }

                }

                packet_data = wl_basic_do_default(&data_length);

                delay_ms(10);

        }

  return 0;

}