/**************************************************************************
 * Title:			oval.c
 *
 * Programmer:		Todd Martin
 *
 * Date:				March, 1999
 *
 * Version:			1
 *
 * Description:	Test the motor.
 *
 **************************************************************************/

/***************************** Constants **********************************/
int COMPASS1 = 0;		/* analog port number */
int COMPASS2 = 1;		/* analog port number */
int MOTOR_JMPR = 0;		/* set to 1 for J4 and set to 0 for J5 */
float STRAIGHT = 40.0;		/* number of degrees to go straight */
float RIGHT = 70.0;
float LEFT = 15.0;
/************************** End of Constants ******************************/

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

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

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

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

/******************************** Main ************************************/
void main()
{

}

void oval()			/*	Go in an oval path	*/
{
	/* A wait routine that allows Tracker to go forward 5 feet */
	/* 5 feet = rate (ft/s) * wait time
	servo_deg1(STRAIGHT);
	motor(MOTOR_JMPR, 100.0);	/* move forward at full speed */
	wait(2000);			/* 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 */
	servo_deg1(LEFT);
	motor(MOTOR_JMPR, 50.0);			/* run motor at half speed */
	wait(2000);			/* wait for ~2 sec */

	/* A wait routine that allows Tracker to go forward 2 feet */
	/* 2 feet = rate (ft/s) * wait time
	servo_deg1(STRAIGHT);
	motor(MOTOR_JMPR, 100.0);	/* move forward at full speed */
	wait(1000);			/* wait for ~1 sec */

	/* A wait routine that allows Tracker to turn 90 degrees in an arc */
	/* (.5 * pi * radius (in)) = rate (ft/s) * wait time */
	servo_deg1(LEFT);
	motor(MOTOR_JMPR, 50.0);			/* run motor at half speed */
	wait(2000);			/* wait for ~2 sec */

	/* A wait routine that allows Tracker to go forward 5 feet */
	/* 5 feet = rate (ft/s) * wait time
	servo_deg1(STRAIGHT);
	motor(MOTOR_JMPR, 100.0);	/* move forward at full speed */
	wait(2000);			/* 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 */
	servo_deg1(LEFT);
	motor(MOTOR_JMPR, 50.0);			/* run motor at half speed */
	wait(2000);			/* wait for ~2 sec */

	/* A wait routine that allows Tracker to go forward 2 feet */
	/* 2 feet = rate (ft/s) * wait time
	servo_deg1(STRAIGHT);
	motor(MOTOR_JMPR, 100.0);	/* move forward at full speed */
	wait(1000);			/* wait for ~1 sec */

	/* A wait routine that allows Tracker to turn 90 degrees in an arc */
	/* (.5 * pi * radius (in)) = rate (ft/s) * wait time */
	servo_deg1(LEFT);
	motor(MOTOR_JMPR, 50.0);			/* run motor at half speed */
	wait(2000);			/* 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 */

void initialize()
{
	init_servos();
	motor(MOTOR_JMPR, 0.0);		/* stop motor */
}

void init_servos()
{
	servo_on();
	servo_deg0(STRAIGHT);		/* straighten the steering servo */
}

void wait(int milli_seconds)
{
	long timer_a;

	timer_a = mseconds() + (long) milli_seconds;
	while (timer_a > mseconds()) 
	{
		defer();
	}
}
/***************************** End of Main ********************************/