Colony

#include "highways.h"

#include "lineDrive.h"

#include <dragonfly_lib.h>

extern int state[5];

static int count =0;

static long average = 0;

void driveHighway( void ){

        doDrive(200);

        int range = range_read_distance(IR2);

        usb_puti(range);

        usb_puts(" : ");

        usb_puti(average);

        usb_puts("\r\n");

        if(range == -1){

                return;

        }

        count ++;

        average += range;

        if(count >= COUNT_MAX){

                if(state[0] != MERGELEFT){

                        orb1_set_color(ORANGE);

                        if(average / COUNT_MAX < PASS_DISTANCE){

                                merge(ILEFT);

                                usb_puts("MERGE \r\n");        

                        }

                }

                count =0;

                average = 0;

        }

        //else orb1_set_color(GREEN);

}