Colony

#include "lineDrive.h"

int state[5];

int stateCounter;

int stateLength;

int stopped=1;

void lineDrive_init()

{

        lineFollow_init();

        for(int i=0; i<5; i++)state[i]=0;

        stateCounter=0;

        stateLength=0;

        stopped=0;

}

int doDrive(int speed)

{

        if(stopped)

        {

                motor_l_set(FORWARD, 0);

                motor_r_set(FORWARD, 0);

                return NORMAL;

        }

        int code;

        switch(state[0])

        {        

        case ISTRAIGHT:

                code = lineFollow(speed);

                if(code==INTERSECTION)

                {

                        for(int i=0; i<4; i++) state[i]=state[i+1];

                        state[4]=0;

                        if(state[0]==0)stateCounter++;

                        break;

                }

                else if(code==NOBARCODE) return NORMAL;

                return code;

        case ILEFT:

                code = turnLeft();

                if(code==0)

                {

                        state[0]=0;

                        stateCounter++;

                }

                break;

        case IRIGHT:

                code = turnRight();

                if(code==0)

                {

                        state[0]=0;

                        stateCounter++;

                }

                break;

        case MERGELEFT:

                code = mergeLeft();

                if(code==0)

                {

                        state[0]=0;

                        stateLength=0;

                        return FINISHED;

                }

                return NORMAL;

        case MERGERIGHT:

                code = mergeRight();

                if(code==0)

                {

                        state[0]=0;

                        stateLength=0;

                        return FINISHED;

                }

                return NORMAL;

        default:

                return LOST;

        }

        if(stateCounter>=stateLength)

        {

                stateCounter=stateLength=0;

                return FINISHED;

        }

        return NORMAL;

}

void start(void){stopped=0;}

void stop(void){stopped=1;}

int merge(int dir)

{

        if(stateLength!=0)return ERROR;

        stateLength++;

        state[0]=(dir==ILEFT ? MERGELEFT : MERGERIGHT);

        return NORMAL;

}

int turn(int type, int dir)

{

        if(stateLength!=0)return ERROR;

        if(dir==IRIGHT){stateLength++; state[1]=IRIGHT; return NORMAL;}

        if(dir==IUTURN){stateLength+=2;  state[1]=state[2]=ILEFT; return NORMAL;}

        if(dir==ISTRAIGHT && type==SINGLE){stateLength++; state[1]=ISTRAIGHT; return NORMAL;}

        if(dir==ISTRAIGHT){stateLength+=2; state[1]=state[2]=ISTRAIGHT; return NORMAL;}

        //must be left turn

        if(type==SINGLE){stateLength++; state[1]=ILEFT; return NORMAL;}

        if(type==DOUBLE){stateLength+=3;state[1]=state[3]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;}

        if(type==ON_RAMP){stateLength+=2; state[1]=ILEFT; state[2]=ISTRAIGHT; return NORMAL;}

        if(type==OFF_RAMP){stateLength+=2; state[1]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;}

        //Should never get here

        return ERROR;

}