Colony » Colony Scout » Colony Scout OS

 * @file lineDrive.cpp

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

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

void lineDrive_init()

#ifndef _LINE_DRIVE_

#define _LINE_DRIVE_

class lineDrive : Behavior{

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

	void lineDrive_init(void);

	int turn(int type, int dir);

}

/**

 * @file lineFollow.cpp

 * @defgroup lineFollwing Line Following

 *

 * Takes care of following a line. Running this program is done by calling the

 * init() function and then the lineFollow(speed) command.  However, direct use

 * of this class is discouraged as its behavior is used by lineDrive.cpp, which

 * extends this class to provide behavior functionality.

 *

 * @author Dan Jacobs and the Colony Project

 * @date 11-1-2010

 */

#include "lineFollow.h"

/**

 * Helps with debugging.

 * 0 - no debug output.

 * 1 - print out buckets

 * 2 - print out line sensor readings

 */

#define DBG_LINEFOLLOW 2

//! Anything lower than this value is white

int GREY_THRESHOLD = 300;

//! Anything higher than this value is black

int BLACK_THRESHOLD = 750;

int countHi = 0;

int countLo = 0;

int maxAvg, avg;

int duration[2] = {0};

int lastColor[2] = {0};

char isReset[2] = {1};

int lastReadings[2][ NUM_READINGS ] = {{0}};

int lastReadingsPtr[2] = {0};

int numLast[2][4] = { {0, 0, 0, NUM_READINGS}, {0, 0, 0, NUM_READINGS} };

int bitColor[2] = {0};

int turnDistance=0;

//! Counts the number of full line readings before we determine an intersection

int intersectionFilter=0;

//! Keeps track of where the encoder of one motor started, for use in turns.

int encoderStart = -1;

int encoderReset = 0;   // 0 if encoderStart has no value set

void lineFollow::init()

{

    int i, j, curReading;

    int lowGrey = 1000, highGrey = 0, lowBlack = 1000,

        highBlack = 0;

	analog_init(0);

    encoders_init();

	lost = 0;

	intersectionFilter=0;

    for(i=0; i<2; i++)

    {

        for(j=0; j<NUM_READINGS; j++)

        {

            lastReadings[i][j] = BAD_READING;

        }

        isReset[i] = 1;

    }

    // Calibrate thresholds

    orb_set_color(YELLOW);

    delay_ms(2000);

    orb_set_color(BLUE);

    for(i=0; i<100; i++)

    {

        curReading = read_line(LEFT_SENSOR + 6);

        if(curReading < lowGrey)

            lowGrey = curReading;

        if(curReading > highGrey)

            highGrey = curReading;

        delay_ms(20);

    }

    orb_set_color(YELLOW);

    delay_ms(2000);

    orb_set_color(GREEN);

    for(i=0; i<100; i++)

    {

        curReading = read_line(LEFT_SENSOR + 6);

        if(curReading < lowBlack)

            lowBlack = curReading;

        if(curReading > highBlack)

            highBlack = curReading;

        delay_ms(20);

    }

    orbs_set(0,0,0,0,0,0);

    GREY_THRESHOLD = lowGrey / 2;

    BLACK_THRESHOLD = (highGrey + lowBlack) / 2;

    usb_puts("Grey: ");

    usb_puti(lowGrey);

    usb_puts(", ");

    usb_puti(highGrey);

    usb_puts("\nBlack: ");

    usb_puti(lowBlack);

    usb_puts(", ");

    usb_puti(highBlack);

    usb_puts("\nThresholds: ");

    usb_puti(GREY_THRESHOLD);

    usb_puts(", ");

    usb_puti(BLACK_THRESHOLD);

    usb_puts("\n\n");

    delay_ms(1500);

}

/** 

 * Follows a line at the given speed.

 * @param speed The speed with which to follow the line.

 */

int lineFollow(int speed)

{

	int colors[5];

	int position;

	updateLine(colors);

	position = lineLocate(colors); 	

	//not on line

	if(position == NOLINE)

	{

		if(lost++ > 20)

		{

			orb2_set_color(GREEN);

			motors_off();

			return LINELOST;

		}

	}

	else if(position == FULL_LINE)

	{

		if(intersectionFilter++ > 4)

		{

			orb2_set_color(RED);

			barCodePosition[0]=0;

            barCodePosition[1]=0;

			disableBarCode=50;

		}

	}

	//on line

	else

	{

		position*=30;

		orb2_set_color(ORB_OFF);

		motorLeft(min(speed+position, 255));

		motorRight(min(speed-position, 255));

		lost=0;

		intersectionFilter=0;

	}

}

/**

 * Turns left at a cross of two lines.  Assumes that we are at lines in a cross

 * pattern, and turns until it sets straight on the new line.

 * @return 0 if turn finishes otherwise return 1

 */

int turnLeft()

{

        motors->set_sides(-20, 20, MOTOR_ABSOLUTE);

        if(!encoderReset)

        {

            encoderStart = encoder_get_x(RIGHT);

            encoderReset = 1;

        }

        if(encoder_get_x(RIGHT) < encoderStart)

        {

            encoderStart = 0;

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

            delay_ms(2000);

        }

        if(encoder_get_x(RIGHT) - encoderStart > 300)

        {

            encoderReset = 0;

            return 0;

        }

        return 1;

}

/**

 * Turns right at a cross of two lines.  Assumes that we are at lines in a cross

 * pattern, and turns until it sets straight on the new line.

 * @return 0 if the turn finishes otherwise return 1

 */

int lineFollow::turnRight()

{

	/// Set motors using new motors functions

	/// motor_r_set(BACKWARD, 200);

	/// motor_l_set(FORWARD, 200);

	int colors[5];

	updateLine(colors);

    int position = lineLocate(colors);

    if(position>2 || position<-2)

		turnDistance++;

    if(turnDistance>1) 

	{

		if(position<3 && position>-3)

		{

			turnDistance = 0;

			 return 0;

		}

	}

	return 1;

}

void updateLine(int* values)

{	

	for(int i = 0; i<5; i++)

		values[i] = (read_line(4-i) < LINE_COLOR ? LWHITE : LBLACK);

}

int lineLocate(int* colors)

{

	int i;

	int wsum = 0;

	int count=0;

	for(i = 0; i<5; i++)

	{

		count += colors[i]/2;

		wsum += (i)*colors[i];

	}

	if(count==0)

		return NOLINE;	

	if(count==5)

		return FULL_LINE;

	return (wsum/count)-4; /// Subtract 4 to center the index around the center.

}

void printBuckets(int i)

{

    int j;

    usb_puts("LP[");

    usb_puti(i);

    usb_puts("]: ");

    usb_puti(lastReadingsPtr[i]);

    usb_puts(", Totals: ");

    for(int j=0; j<=3; j++)

    {

        usb_puts("[");

        usb_puti(j);

        usb_puts("]: ");

        usb_puti(numLast[i][j]);

        usb_puts(" ");

    }

    usb_puts("\t ");

    usb_puts("\n");

}

void addToBuckets(int curColor, int i)

{

    int oldest = lastReadings[i][lastReadingsPtr[i]];

    numLast[i][oldest]--;

    lastReadings[i][lastReadingsPtr[i]] = curColor;

    lastReadingsPtr[i] = (lastReadingsPtr[i]+1) % NUM_READINGS;

    numLast[i][curColor]++;

    if(DBG_LINEFOLLOW == 1)

    {

        printBuckets(i);

    }

}

int min(int x, int y){return x>y ? y : x;}

int max(int x, int y){return x<y ? y : x;}

void motorLeft(int speed)

{

	int tempspeed = 0;

	if ((speed-=127)>=0)

	{	

		tempspeed = 160+speed*95/128;

		motors->set_sides(tempspeed, 0, MOTOR_ABSOLUTE);

	}

	else

	{

		tempspeed = 160+speed*95/128;

		motors->set_sides(-tempspeed, 0, MOTOR_ABSOLUTE);

	}

}

void motorRight(int speed)

{

	int tempspeed = 0;

	if ((speed-=127)>=0)

	{	

		tempspeed = 160+speed*95/128;

		motors->set_sides(0, tempspeed, MOTOR_ABSOLUTE);

	}

	else

	{

		tempspeed = 160+speed*95/128;

		motors->set_sides(0, -tempspeed, MOTOR_ABSOLUTE);

	}

}

#ifndef _LINEFOLLOW_H_

#define _LINEFOLLOW_H_

#include "../Behavior.h"

#define LWHITE			0

#define LGREY			1

#define LBLACK	 		2

#define BAD_READING     3

#define CENTER			3

#define LINELOST		-1

#define INTERSECTION 	-25

#define NOLINE			-50

#define FULL_LINE 		-51

#define NUM_READINGS 20

#define LEFT_SENSOR     1

#define RIGHT_SENSOR    0

class lineFollow : Behavior

{

	public:

		lineFollow(std::string scoutname) : Behavior(scoutname) {};

        /** Actually executes the behavior. */

		void init();

};

/**	lineFollow

 *	Must call lineFollow_init first

 *	Must be called inside a loop

 */

int lineFollow(int speed);

/**	turnLeft turnRight

 *	Must be called inside a loop

 *	returns 0 when complete

 */

int turnLeft(void);

int turnRight(void);

void addToBuckets(int curColor, int i);

void printBuckets();

/**

 * @brief Updates the values stored in the array to white or black based on

 * current sensor readings.

 *

 * @param values The array of five integers to be updated. 

 */

void updateLine(int* values); 

/**

 * @brief Returns an index of the middle of the line based on line readings.

 *

 * Two special return values are possible:

 *   NOLINE if none of the sensors holds a black value, and

 *   FULL_LINE if all of the sensors see black.

 *

 * Otherwise, returns a value from -4 (farthest left) to 4 (farthest right), with

 * 0 the line being centered in the middle.

 *

 * @param colors The array of 5 readings from the line sensor.  Must be either

 *    LWHITE or LBLACK.

 * @return Either a special value or an index from -4 to 4.

 *

 */

int lineLocate(int* colors);

//! A simple function to return the minimum of two integers.

int min(int x, int y);

//! A simple function to return the maximum of two integers.

int max(int x, int y);

/** @todo Alex: I hate these functions, but I'm keeping them so code will still work. But we should delete them sometime. */

/**	motorLeft

 *	Commands the left motor

 *	Cannot be used to stop

 *	0-126 are backward

 *	127-255 are forward

 */

void motorLeft(int speed);

/**	motorRight

 *	Commands the right motor

 *	Cannot be used to stop

 *	0-126 are backward

 *	127-255 are forward

 */

void motorRight(int speed);

/**	lost

 *	Internal counter to detect if the line was lost

 */

int lost;

int onLine(void);

#endif