Colony

#include "highways.h"

#include "lineDrive.h"

#include <dragonfly_lib.h>

static int counti =0;

static long averagei = 0;

static int states = 0;

int canMerge = 1;

void highwayFSM(){

        switch(states){

        case 0: // Normal Drive

                doDrive(225);

                int range = range_read_distance(IR2);

                usb_puti(range);

                       usb_puts(" : ");

                usb_puti(averagei);

                usb_puts("\r\n");

                orb1_set_color(PURPLE);

                       if(range == -1){

                        return;

                }

                counti ++;

                averagei += range;

                if(counti>= COUNT_MAX){

                        if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge)

                                states = 1;

                        averagei = 0;

                        counti = 0;

                }

                break;

                case 1:

                        merge(ILEFT);

                        states = 2;

                        canMerge = 0;

                case 2:

                        if(doDrive(160) == FINISHED) states = 0;

                        orb1_set_color(BLUE);

                        break;

        }

}

/*

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

}

*/