/**************************************************************************
 * Title:			control1.c
 *
 * Programmer:		Todd Martin
 *
 * Date:				March, 1999
 *
 * Version:			1
 *
 * Description:	Collision avoidance using two front IR sensors.
 *
 **************************************************************************/

/****************************** Includes **********************************/
#include <me11.h>
#include <vectors.h>
/*************************** End of Includes ******************************/

/***************************** Constants **********************************/
#define LEFT		2000	/* might want to use an array to give more control */
#define RIGHT		4000	/* e.g. RIGHT(20), these values need to be verified */
#define STRAIGHT	0		/* find values for one degree increments */
/************************** End of Constants ******************************/

/***************************** Prototypes *********************************/

/************************** End of Prototypes *****************************/

/******************************* Globals **********************************/

/**************************** End of Globals ******************************/

/******************************** Main ************************************/
int main(void)
{
	/* Local variables */
	int rval, lval;		/* variables for the front IR sensors */
	int i;

	init_analog();
	init_ir();
	init_motors();
	init_servos();		/* Initialize everything needed to run the servos */

	/* Initialize port registers */

	while(1)
	{
/*	Object avoidance to be implemented later.
		/* Check the front IR sensors and turn if needed */
		rval = ir_value(6);		/* I might not use 6 and 7 like the example */
		lval = ir_value(7);

		if (rval > 100)
			steer = RIGHT;
		else
			steer = STRAIGHT;
		if (lval > 100)
			steer = LEFT;
		else
			steer = STRAIGHT;

		servo(STEERING_SERVO, steer);
*/

	/*	Go in an oval path	*/
		/* A wait routine that allows Tracker to go forward 5 feet */
		/* 5 feet = rate (ft/s) * wait time
		servo(STEERING_SERVO, STRAIGHT);
		for (i = 0; i < 25000; i++);	/* wait for ~2 sec */

		/* A wait routine that allows Tracker to turn 90 degrees in an arc */
		/* (.5 * pi * radius (in)) = rate (ft/s) * wait time */
		/* run motor at half speed */
		servo(STEERING_SERVO, LEFT);
		for (i = 0; i < 25000; i++);	/* wait for ~2 sec */

		/* A wait routine that allows Tracker to go forward 2 feet */
		/* 2 feet = rate (ft/s) * wait time
		servo(STEERING_SERVO, STRAIGHT);
		for (i = 0; i < 25000; i++);	/* wait for ~2 sec */

		/* A wait routine that allows Tracker to turn 90 degrees in an arc */
		/* (.5 * pi * radius (in)) = rate (ft/s) * wait time */
		/* run motor at half speed */
		servo(STEERING_SERVO, LEFT);
		for (i = 0; i < 25000; i++);	/* wait for ~2 sec */

		/* A wait routine that allows Tracker to go forward 5 feet */
		/* 5 feet = rate (ft/s) * wait time
		servo(STEERING_SERVO, STRAIGHT);
		for (i = 0; i < 25000; i++);	/* wait for ~2 sec */

		/* A wait routine that allows Tracker to turn 90 degrees in an arc */
		/* (.5 * pi * radius (in)) = rate (ft/s) * wait time */
		/* run motor at half speed */
		servo(STEERING_SERVO, LEFT);
		for (i = 0; i < 25000; i++);	/* wait for ~2 sec */

		/* A wait routine that allows Tracker to go forward 2 feet */
		/* 2 feet = rate (ft/s) * wait time
		servo(STEERING_SERVO, STRAIGHT);
		for (i = 0; i < 25000; i++);	/* wait for ~2 sec */

		/* A wait routine that allows Tracker to turn 90 degrees in an arc */
		/* (.5 * pi * radius (in)) = rate (ft/s) * wait time */
		/* run motor at half speed */
		servo(STEERING_SERVO, LEFT);
		for (i = 0; i < 25000; i++);	/* wait for ~2 sec */

	/*	Turn around	*/
		/* backup and turn to the right */

	/*	Go in an oval path in the opposite direction	*/
		/* go forward about 3 feet instead of 2, then continue the normal pattern */

/***************************** End of Main ********************************/