/**************************************************************************
 * Title:			Control1.c
 *
 * Programmer:		Todd Martin
 *
 * Date:				March, 1999
 *
 * Version:			1
 *
 * Description:	Test the motors and compass sensor.  Written in IC.
 *
 **************************************************************************/

/***************************** Constants **********************************/
int COMPASS1 = 0;				/* analog port number for the sin curve*/
int COMPASS2 = 1;				/* analog port number for the cos curve*/
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;			/* max amount wheels can turn right */
float LEFT = 15.0;			/* max amount wheels can turn left */
/************************** End of Constants ******************************/

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

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

/******************************* Globals **********************************/
int angle;					/* the angle from 0 to 360 showing the direction */
int total_dist;			/* keep track of the wheel encoder counts */
/*int turn_comp;				 returns 1 if the desired angle has been reached */
int i;						/* a counter for loops */
/**************************** End of Globals ******************************/

/******************************** Main ************************************/
void main()
{
	initialize();
	speed_test();
	motor(MOTOR_JMPR, 0.0);		/* stop motor */
}
/***************************** End of Main ********************************/

/****************************** Functions *********************************/
void initialize()
{
	init_servos();
	motor(MOTOR_JMPR, 0.0);		/* stop motor */
	total_dist = 0;				/* clear the distance counter */
/*	dir_angle();	*/				/* returns the current angle */
										/* scan the surroundings */
}

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

void straighten()
{
	/* stop turning by smoothly straightening the wheels */
	/* follows turn() */
	servo_deg1(STRAIGHT);
}

void speed_test()
{
	straighten();
	motor(MOTOR_JMPR, 100.0);
	wait(5000);
}

void wait(int milli_seconds)
{
	long timer_a;

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