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

}

#ifndef _LINE_DRIVE_

#define _LINE_DRIVE_

#include "lineFollow.h"

#define DOUBLE		0

#define SINGLE		1

#define ON_RAMP		2

#define OFF_RAMP	3

#define ISTRAIGHT	0

#define ILEFT		1

#define IRIGHT		2

#define IUTURN		3

#define MERGELEFT	4

#define MERGERIGHT	5

#define NORMAL		-1

#define FINISHED	-2

#define LOST		-3

#define ERROR		-4

void lineDrive_init(void);

int doDrive(int speed);

void start(void);

void stop(void);

int merge(int dir);

int turn(int type, int dir);

#endif