Colony

/**

 * @file lineDrive.c

 *

 * Provides functions to implement line driving behavior.  This program extends

 * the behavior of the line-following program by following lines automatically

 * and implementing behaviors to deal with commands passed to lineDrive.

 *

 * @author Dan Jacobs

 * @date 11-1-2010

 */

#include "lineDrive.h"

int state[5];       //! Stores a queue of sub-commands to be executed

int stateCounter;

int stateLength;

//! Whether lineDrive is currently paused. Set to 0 on initialization.

int stopped=1;

/**

 * Starts the line following procedure. Must be called before other

 * line-following functions will work.  This function essentially resets the

 * state of line-following.

 */

void lineDrive_init()

{

        lineFollow_init();

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

        stateCounter=0;

        stateLength=0;

        stopped=0;

}

/**

 * Follows a line and executes whatever command is next on the queue.

 * @param speed The speed with which to drive along the line.

 */

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;

}

/** Starts the line-drive process if paused. */

void start(void){stopped=0;}

/** Pauses the line-drive process. Default is started. */

void stop(void){stopped=1;}

/**

 * Defines a merge command in the direction specified.  A merge is a switch

 * of lanes.

 * @param dir Left or right, defined by ILEFT or IRIGHT

 */

int merge(int dir)

{

        if(stateLength!=0)return ERROR;

        stateLength++;

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

        return NORMAL;

}

/**

 * Executes an intersection turn where the intersection type is specified by the

 * parameters. 

 * @param type A valid defined intersection type

 * @param dir The direction to turn at the intersection

 */

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;

}