Colony

#include <dragonfly_lib.h>

#include <encoders.h>

int main(void)

{

        /* initialize components, set wireless channel */

        dragonfly_init(ALL_ON);

        encoders_init();

  //Replace the next two lines with your own code

  //You can add as many lines as you want

  uint8_t rangeNumber[] = {IR1,IR2,IR3,IR4,IR5}; //0 indexed

  uint8_t cIndex;

  //initial setup (IR1)

  cIndex=1;

  orb1_set_color(BLUE);

  orb2_set_color(GREEN);

  int lastDir = FORWARD;

  int travel=0;

  encoder_rst_dx(LEFT);

  encoder_rst_dx(RIGHT);

  while (1) {

  if (lastDir == FORWARD) {

        lastDir=BACKWARD;

        motor_l_set(lastDir,200);

        motor_r_set(lastDir,200);  

  } else {

        lastDir=FORWARD;

        motor_l_set(lastDir,200);

        motor_r_set(lastDir,200);  

  }

        while(travel<660) {

                usb_puti(range_read_distance(rangeNumber[cIndex]));

                usb_putc('\r');

                /**

                 *if (button1_click()) {

                 *        switch (cIndex){ //button1 chooses a color channel for "editing"

                 *                case 0: //IR1

                 *                        orb1_set_color(RED);

                 *                        orb2_set_color(RED);

                 *                        cIndex=1;

                 *                        break;

                 *                case 1: //IR2

                 *                        

                 *                        orb1_set_color(GREEN);

                 *                        orb2_set_color(GREEN);

                 *                        cIndex=2;

                 *                        break;

                 *                case 2: //IR3

                 *                        

                 *                        orb1_set_color(BLUE);

                 *                        orb2_set_color(BLUE);

                 *                        cIndex=3;

                 *                        break;

                 *                case 3: //IR4

                 *                        orb1_set_color(YELLOW);

                 *                        orb2_set_color(YELLOW);

                 *                        cIndex=4;

                 *                        break;        

                 *                case 4: //IR5

                 *                        orb1_set_color(BLUE);

                 *                        orb2_set_color(GREEN);

                 *                        cIndex=0;

                 *                        break;

                 *                default: //undefined

                 *                        return 0;

                 *                        break;

                 *        }

                 *        delay_ms(50); //allows button press to release

                 *}

                 */

                delay_ms(50); //refresh frequency: 1000/(50ms) refresh rate

                travel=abs((encoder_get_dx(LEFT)+encoder_get_dx(RIGHT))/2);

        }

        //reset travel variables

        travel=0;

        encoder_rst_dx(LEFT);

        encoder_rst_dx(RIGHT);

        }        

  //this tell the robot to just chill out forever. don't put anything after this

  while(1);

        return 0;

}