Colony » Colony Scout » Colony Scout OS

/**

 * @file lineDrive.cpp

 *

 * 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.

 *

 * Specific implementation for the colony scout robots in a warehouse behaviour

 * setting.

 * 

 * @author James Carroll（キヤロル）

 * Cannabalized from Dan Jacobs code.

 * @date 2-6-2012

 */

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

	{

		motors->set_sides(0,0,ABSOLUTE);

		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;

		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;

}

/**

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

	//turns that don't depend on what kind of road your on

        if(dir==ISTRAIGHT)

        {

                stateLength++;

                state[1]=ISTRAIGHT;

                return NORMAL;

        }

	if(dir==IUTURN)

    	{

        	stateLength+=2;

        	state[1]=state[2]=ILEFT;

        	return NORMAL;

    	}

	if(type==ON_ROAD)

	{

		if(dir==IRIGHT){

			stateLength++;

			state[1] = IRIGHT;

			return NORMAL;

		}

		if(dir==ILEFT){

			stateLength += 2;

			state[1] = ILEFT;

			state[2] = ISTRAIGHT;

			return NORMAL

		}

	}

	if(type==OFF_ROAD)

	{

		if(dir==IRIGHT){

			stateLength++;

			state[1]=IRIGHT;

			return NORMAL;

		}

		if(dir==ILEFT){

			stateLength+=2;

			state[1]=ISTRAIGHT;

			state[2]=ILEFT;

		}

	}

	//Should never get here

	return ERROR;

}

#ifndef _LINE_DRIVE_

#define _LINE_DRIVE_

#include "lineFollow.h"

/*

 * Defines whether your making a turn from the main two-way road

 * or from a one-way sub road.

*/

#define ON_ROAD		0

#define OFF_ROAD	0

/*

 * The type of turn you are making from the road you are on.

*/

#define ISTRAIGHT	0

#define ILEFT		1

#define IRIGHT		2

#define IUTURN		3

#define NORMAL		-1

#define FINISHED	-2

#define LOST		-3

#define ERROR		-4

class lineDrive : Behavior{

	lineDrive (String scoutname): Behavior(scoutname){};

	run();

	void lineDrive_init(void);

	int doDrive(int speed);

	void start(void);

	void stop(void);

	int turn(int type, int dir);

}

#endif