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 highwayStart(){

        canMerge = 1;

}

int changeLanes(){

        if(counti<10000){

                counti++;

                motor_r_set(FORWARD,232);

                motor_l_set(FORWARD,200);

                return 0;

        }

        int colours[5];

        updateLine(colours);

        int pos = lineLocate(colours);

        if(pos >= 3 || pos <= -3){

                motor_r_set(FORWARD,187);

                motor_l_set(FORWARD,190);

                return 0;

        }

        counti = 0;

        return 1;

}

int highwayFSM(){

        switch(states){

        case 0: // Normal Drive

                {

                int theCode = doDrive(205);

                if(theCode != NORMAL && theCode >= 0)

                        return theCode;

                int range = range_read_distance(IR3);

                usb_puti(range);

                       usb_puts(" : ");

                usb_puti(averagei);

                usb_puts("\r\n");

                orb1_set_color(CYAN);

                       if(range == -1){

                        range = PASS_DISTANCE+50;

                }

                counti ++;

                averagei += range;

                if(counti>= COUNT_MAX){

                        if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge)

                                states = 1;

                        averagei = 0;

                        counti = 0;

                }

                break;

                }

        case 1:

                        states = 2;

                        canMerge = 0;

        case 2:

                        if(changeLanes()){

                           states = 0;

                           orb1_set_color(GREEN);

                        }

                        else orb1_set_color(YELLOW);

                        break;

        }

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

}

*/