/**************************************************************************
 * 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();
	pattern();
	motor(MOTOR_JMPR, 0.0);		/* stop motor */
}
/***************************** End of Main ********************************/

/****************************** Functions *********************************/
void bump()			/* simulates a bump sensor */
{
	/* if the motor is moving, but the shaft encoder says Tracker is not moving */
	/* stop and move in the opposite direction.
}

void dir_angle()
{
	int sine;
	int cosine;
	int upper_line;
	int lower_line;
	int angle_res

	sine = analog(COMPASS1);
	cosine = analog(COMPASS2);

	upper_line = 200;			/* when sine and cosine are equal high. verify with oscope */
	lower_line = 55;			/* when sine and cosine are equal low. verify with oscope */

	angle_res = ((upper_line - lower_line) / 90);

	if (sine > upper_line) 
	{
		/* cosine measures from 45 to 135 degrees */
		angle = (((cosine - lower_line) / angle_res) + 45);
	}
	if (cosine < lower_line)
	{
		/* sine measures from 135 to 225 degrees */
		angle = (((sine - lower_line) / angle_res) + 135);
	}
	if (sine < lower_line)
	{
		/* cosine measures from 225 to 315 degrees */
		angle = (((cosine - lower_line) / angle_res) + 225);
	}
	if (cosine > upper_line)
	{
		/* sine measures from 315 to 45 degrees */
		angle = (((sine - lower_line) / angle_res) + 315);
	}
	if (angle >= 360)
	{
		angle = angle - 360;		/* keep the values of angle between 0 and 360 */
	}
}

void distance()					/* measure the distance traveled */
{
	/* there are 6 counts per revolution */
	/* for each pulse of the wheel encoder */
		total_dist = total_dist + 1;
}

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 pattern()		/* Go in a pattern using the compass as a guide */
{
	for (i = 0; i = 1; i++)
	{
		straighten();
		motor(MOTOR_JMPR, 100.0);
		wait(6000);

		turn(90);

		straighten();
		motor(MOTOR_JMPR, 100.0);
		wait(3000);

		turn(90);
	}
}

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

void speed_test()			/* Tracker moves about 3.6 inches/second, varies with battery power */
{
	straighten();
	motor(MOTOR_JMPR, 100.0);
	wait(5000);
}

void turn(int angle_chg)	/* notify when I reach the specified angle change */
{
	/* monitor compass to see when I reach the desired angle */
/*	int angle_comp = 0;		 1 if the angle was adjusted to be between 0 and 360 */
/*	int angle_comp_dir;		 specifies the direction in which compensation was applied */
	int new_angle;
	

	/* calculate what the new angle should be */
	dir_angle();								/* returns angle */
	new_angle = angle + angle_chg;
	if (new_angle >= 360)
	{
		new_angle = new_angle - 360;
/*		angle_comp = 1;
		angle_comp_dir = 1; */
	}
	if (new_angle < 0)
	{
		new_angle = new_angle + 360;
/*		angle_comp = 1;
		angle_comp_dir = 0; */
	}

	while (angle != new_angle)
	{
		/* logic that determines turning smoothness, speed, and amount */
		if (angle_chg < 0) 	/* turn left */
		{
			servo_deg1(LEFT);
			motor(MOTOR_JMPR, 50.0);			/* run motor at half speed */
		}
		else		/* turn right */
		{
			servo_deg1(RIGHT);
			motor(MOTOR_JMPR, 50.0);			/* run motor at half speed */			
		}
		dir_angle();								/* check the current angle */
	}
}

void wait(int milli_seconds)
{
	long timer_a;

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