Colony

/** driver for bfs swarming

        execute bfs behavior

*/

#define COLOR_DETECT // comment out if color detect hardware is unavailable

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_token_ring.h>

#include <dio.h>

#include <analog.h>

#include "bfs_fsm.h"

#include "smart_run_around_fsm.h"

//A6

#define RED_LED PIN_ADC4

//A8

#define GREEN_LED PIN_ADC5

//A7

#define BLUE_LED PIN_ADC6

// COLOR_NOTHING {255,255,255}

#define COLOR_WALL 2 // ratio of blue/green

#define COLOR_GREEN 1

#define SWARM_GROUP   15

#define SWARM_ACTION  16

/* swarm wireless packet handlers */

void swarm_receive (char type, int source, unsigned char* packet,

                          int length);

PacketGroupHandler swarmHandler = {SWARM_GROUP, NULL, NULL, 

                                     &swarm_receive, NULL};

int main(void) {

  // enable everything

  dragonfly_init(ALL_ON);

  orb_enable();

  orb_init();

  orb_set_color(PURPLE);

  wl_init();

  wl_set_channel(0xF); // set wireless channel

  // register swarm packet

  wl_register_packet_group(&swarmHandler);

  wl_token_ring_register();

  wl_token_ring_join(); // join token ring

  usb_init();

  usb_puts("start\n\r");

#ifdef COLOR_DETECT

  /* set up color detect */

  int rgb[3] = {0,0,0};

  int rgb_old1[3] = {-1,0,0};

  int rgb_old2[3] = {-1,0,0};

  int rgb_avg[3] = {0,0,0};

        //long int distance_red;

        //long int distance_green;

        //long int distance_blue;

  int ratio_gb;

        //int blue[3] = {255,245,249};

        //int red[3] = {190,122,126};

        //int green[3] = {118, 80, 86};

  analog_init(ADC_START);

#else

  run_around_init();

#endif

  int object = 0;

  while(1) {

    wl_do(); // do wireless

#ifdef COLOR_DETECT

    // need color sensor code here

    digital_output(BLUE_LED, 1);

                delay_ms(5);

                analog_stop_loop();

                rgb[2] = analog8(AN9);

                analog_start_loop();

                digital_output(BLUE_LED,0);

                digital_output(RED_LED, 1);

                delay_ms(5);

                analog_stop_loop();

                rgb[0] = analog8(AN9);

                analog_start_loop();

                digital_output(RED_LED, 0);

                digital_output(GREEN_LED,1);

                delay_ms(5);

                analog_stop_loop();

                rgb[1] = analog8(AN9);

                analog_start_loop();

                digital_output(GREEN_LED,0);

    // check with older values

    if (rgb_old1[0] != -1 && rgb_old2[0] != -1) {

      rgb_avg[0] = (rgb[0]+rgb_old1[0]+rgb_old2[0])/3;

      rgb_avg[1] = (rgb[1]+rgb_old1[1]+rgb_old2[1])/3;

      rgb_avg[2] = (rgb[2]+rgb_old1[2]+rgb_old2[2])/3;

    } else {

      rgb_avg[0] = rgb[0];

      rgb_avg[1] = rgb[1];

      rgb_avg[2] = rgb[2];

    }

    if (rgb_old1[0] == -1) {

      rgb_old1[0] = rgb[0];

      rgb_old1[1] = rgb[1];

      rgb_old1[2] = rgb[2];

    } else {

      if (rgb_old1[0] < rgb[0]+20 

        && rgb_old1[0] > rgb[0]-20) {

        rgb_old1[0] = rgb[0];        

        rgb_old2[0] = rgb_old1[0];

      }

      if (rgb_old1[1] < rgb[1]+20 

        && rgb_old1[1] > rgb[1]-20) {

        rgb_old1[1] = rgb[1];

        rgb_old2[1] = rgb_old1[1];

      }

      if (rgb_old1[2] < rgb[2]+20 

        && rgb_old1[2] > rgb[2]-20) {

        rgb_old1[2] = rgb[2];

        rgb_old2[2] = rgb_old1[2];

      }

    }    

    // check ratio

    ratio_gb = rgb_avg[1] - rgb_avg[2];

    if (ratio_gb < -20)

      ratio_gb = 2;

    else if (ratio_gb > 20)

      ratio_gb = 0;

    else

      ratio_gb = 1;

    if (rgb_avg[0] > 200 && rgb_avg[1] > 200 && rgb_avg[2] > 200) {

      // we think this is open area

      move(170,0);

    } else if (ratio_gb == COLOR_WALL) {

      // we think this is a wall, so turn away

      move(170,90);

      delay_ms(400);

      move(170,0);

    } else if (ratio_gb == COLOR_GREEN) {

      // we think this is a green object, so swarm

      object = 1;

      move(0,0);

    }

    else

      move(170,0); // just go forth

                /*distance_red = 1;

                distance_green = rgb[1] - rgb[0];

                distance_blue = rgb[2] - rgb[0];

                distance_blue = distance_blue * rgb[0]/255;

                distance_green = distance_green * rgb[0]/255;*/

    usb_puts("R:");

                usb_puti(rgb_avg[0]);

                usb_puts("\tG:");

                usb_puti(rgb_avg[1]);

                usb_puts("\tB:");

                usb_puti(rgb_avg[2]);

                usb_puts("\t\tratio_gb:");

    usb_puti(ratio_gb);

    usb_puts("\r\n");

#else

    // do smart run around until find object

    run_around_FSM();

    //move(170,0);

#endif  

    /*usb_puts("R:");

                usb_puti(distance_red);

                usb_puts("\tG:");

                usb_puti(distance_green);

                usb_puts("\tB:");

                usb_puti(distance_blue);

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

    if (object || button2_read()) { // if we find the object 

      // send packet to call for swarm on object

      wl_send_global_packet(SWARM_GROUP,SWARM_ACTION,

        NULL,0,0); 

      move(0,0); // stop

      break; // break loop

    } 

  }

  // object manip goes here

  orb_set_color(YELLOW);

  while(1) {

    wl_do();

  }

  return 0;

}

void swarm_receive (char type, int source, unsigned char* packet,

                          int length)

{

  switch (type)

  {

    case SWARM_ACTION:

      // do swarm action

      bfs_init(source); // set robot_id to find

      while(1) {

        bfs_fsm(); // do bfs

      }

  }

}