/***********************************************************************************************
 * Title:			Control5.c
 *
 * Programmer:		Todd Martin
 *
 * Date:				April, 1999
 *
 * Version:			5
 *
 * Description:	Develop algorithms for following a path.  Written in IC.
 *
 **********************************************************************************************/

/********************************** Persistent Variables ********************************************/
persistent int follow;
persistent int follow_pid;		/* pid for follow_path() */
persistent int dist[21];		/* allocate space for record path */
persistent int dir[21];			/* allocate space for record path */
persistent float deg[21];		/* allocate space for record path */
persistent int path_index;
persistent int mem_test;		/* used in test_memory() */
/******************************* End of Persistent Variables *************************************/

/*************************************** Constants ********************************************/
int COMPASS1 = 0;				/* analog port number for the cos curve*/
int COMPASS2 = 1;				/* analog port number for the sin 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 = 60.0;			/* 70 max amount wheels can turn right */
float LEFT = 20.0;			/* 15 max amount wheels can turn left */
int IR_THRESHOLD = 95;		/* the value used in determining if an object has been detected */
int ODOM_THRESHOLD = 120;	/* the value used in determining if the wheel is turning */
int ODOMETER = 2;				/* analog port number for the wheel encoder ir sensor */
int IR_FR_LFT = 3;			/* analog port number for the front left ir sensor */
int IR_FR_RGHT = 4;			/* analog port number for the front right ir sensor */
int IR_BK_LFT = 5;			/* analog port number for the back left ir sensor */
int IR_BK_RGHT = 6;			/* analog port number for the back right ir sensor */
int SONAR = 7;					/* analog port number for the sonar sensor */
float DEG1_CHG = 1.0;		/* amount deg1 is changed in turn() */
/************************************ End of Constants ****************************************/

/***************************************** Globals ********************************************/
int angle;							/* the angle from 0 to 360 showing the direction */
int avg_angle;						/* an average of multiple angle readings */
int new_angle;
int last_angle;
int total_dist;					/* keep track of the wheel encoder counts */
int output1;
int output2;
int range1;
int range2;
int upper_line = 155;			/* when output1 and output2 are equal high */
int lower_line = 108;			/* when output1 and output2 are equal low */
float angle_resolution;
float angle_res;
int forward;
int reverse;
int turn_right;
int turn_left;		
int record;
int rst;	
int straight;
float deg1;							/* keeps track of the current setting for servo_deg1() */
float j;								/* used to turn in small increments */
int record_pid;					/* pid for record_mode() */
int follow_active;				/* set to 1 when follow_path() is running as a process */
int odom;							/* keeps track of pulse levels for the odometer */
int obj_fwd_left;
int obj_fwd_right;
int reverse_dist;
int straight_lvl;
int detect;							/* set to 1 when an obstacle is detected */
long timer;							
float servo_angle;				/* set to 1 when the wheels have turned the specified amount */
int ref_angle;
int avoiding;
int turn_angle;
int turn_dist;
int straight_dist;
int locate;							/* when set to 1 it tells the program to locate the path */
float turn_direction;
int step;
int ref_dist;
int wall;
int angle_chg;
/************************************** End of Globals ****************************************/

/****************************************** Main **********************************************/
void main()
{
	initialize();
	/* start off going straight before turning */
/*	start_process(receive()); */				/* look for data entering the serial port */
/*	receive(); */
	pattern(); 
/*	start_process(distance()); */
/*	start_process(test()); */
/*	test_odometer();  */
/*	test_turning(); */
}
/*************************************** End of Main ******************************************/

/************************************ Program Functions ***************************************/
void test()
{
	motor(MOTOR_JMPR, 100.0);
	while (1)
	{
		if (step == 0)
		{
			avoid();
			if ((detect == 1))
			{
				step = 1;
				avoiding = 1;		/* starting to avoid an object */
/*				distance(); */
				ref_dist = total_dist;
				average_angle(&avg_angle);
				ref_angle = angle;
			}
		}
/*		if ((detect == 0) && (avoiding == 1) && (locate == 0)) */
		else if (step == 1)
		{
			avoiding = 0;
			while ((analog(IR_FR_LFT) > IR_THRESHOLD) || (analog(IR_FR_RGHT) > IR_THRESHOLD))
			{
				/* stay at this point until object is no longer being detected */
			}
/*			straighten(); */
			step = 2;					
		}
/*		if ((avoiding == 0) && (detect == 0) && (locate == 0)) */
		else if (step == 2)
		{
			while (wall == 0)
			{
				if (analog(5) > (116))
				{
					wall = 1;
				}
			}
/*			distance(); */
			turn_dist = total_dist - ref_dist;
			average_angle(&avg_angle);
			turn_angle = avg_angle - ref_angle;
			while ((wall == 1))
			{
				straighten();			/* continue going straight until passed the object */
				motor(MOTOR_JMPR, 100.0);
/*				avoid(); */
/*				wall_follow(); */
				if (analog(5) < (116))
				{
					wall = 0;
				}
			}
/*			distance(); */
			straight_dist = total_dist - turn_dist;
			locate = 1;					/* when set to 1 it tells the program to locate the path */
			step = 3;
		}
/*		if (locate == 1) */
		else if (step == 3)
		{
			write_int(turn_dist);
			write_int(turn_angle);
			move_fwd(turn_dist, turn_angle, turn_direction);
			if (turn_direction == RIGHT)
			{
				turn_direction == LEFT;
			}
			else if (turn_direction == LEFT)
			{
				turn_direction == RIGHT;
			}
			step = 4;					/* go straight again */
		}
		else if (step == 4)
		{
			move_fwd(straight_dist, 0, STRAIGHT);
			step = 5;					/* turn back to the original angle */
		}
		else if (step == 5)
		{
			average_angle(&avg_angle);
			angle_chg = avg_angle - ref_angle;
			move_fwd(50, angle_chg, turn_direction);
			straighten();
			motor(MOTOR_JMPR, 0.0);
		}
	} 
}

void average_sensor(int port, int *result)
{
	/* I could try dropping the highest and lowest values then dividing by 8 */
	int sum = 0;
	int i;

	for (i = 0; i < 3; i++) sum += analog(port);

	*result = sum / 3;
}

void average_angle(int *result)		/* runs as a process in record_mode(), but is called otherwise */
{
	int sum = 0;
	int i;

	for (i = 0; i < 2; i++) 
	{
		dir_angle();
		sum += angle;
	}

	*result = sum / 2;
}

void avoid()		/* reads data from the forward IR and sonar and says if an obstacle is detected */
{
	obj_fwd_left = 0;
	obj_fwd_right = 0;
	detect = 0;
	if (analog(IR_FR_LFT) > IR_THRESHOLD) 		/* an obstacle has been detected to the left */
	{
		obj_fwd_left = 1; 		/* robot should turn right */
		/* tell the sonar look right to warn of future obstacles */
	}
	if (analog(IR_FR_RGHT) > IR_THRESHOLD)		/* an obstacle has been detected to the right */
	{
		obj_fwd_right = 1; 	/* robot should turn left */
		/* tell the sonar look left to warn of future obstacles */
	}
/*	if ((obj_fwd_left == 1) && (obj_fwd_right == 1))
	{ */
		/* stop, or slow, the motor and confirm with sonar and judge the distance */
		/* confirm(1); */
		/* once confirmed stop and call back_up() */
/*		motor(MOTOR_JMPR, 0.0);	*/		/* stop before backing up */
/*		detect = 1; */
/* set up a timer. if this happens milliseconds after one sensor returns true then straighten */
/*		if ((mseconds() - timer) < (long) 100)
		{
			straighten();
		}
		reverse = 1;
		back_up();
	} */
/* return this to else if, if I can generate a random direction to turn in the statement above */
	if (obj_fwd_left == 1)
	{
		detect = 1;
		motor(MOTOR_JMPR, 100.0);
/*		servo_deg1(RIGHT); */
		while (deg1 < RIGHT)
		{
			turn(-10, RIGHT);				/* the negative angle tells it to turn right */
			if (analog(IR_FR_LFT) < IR_THRESHOLD)
			{
				break;
			}
		}
		turn_direction = RIGHT;
		timer = mseconds();
	}
	else if (obj_fwd_right == 1)
	{
		detect = 1;
		motor(MOTOR_JMPR, 100.0);
/*		servo_deg1(LEFT); */
		while (deg1 > LEFT)
		{
			turn(10, LEFT);
			if (analog(IR_FR_RGHT) < IR_THRESHOLD)
			{
				break;
			}
		}
		turn_direction = LEFT;
		timer = mseconds();
	} 
}

void back_up()		/* reads data from the back IR and sonar and tells the motors an obstacle is detected */
{
	obj_fwd_left = 0;
	obj_fwd_right = 0;
	motor(MOTOR_JMPR, -100.0);
	while (reverse == 1)
	{
		if ((analog(IR_FR_LFT) < IR_THRESHOLD) && (analog(IR_FR_RGHT) < IR_THRESHOLD)) 
		{ 
			/* continue in reverse for 6 inches or until an object is detected */
			distance(); 
			reverse_dist = total_dist + 6; 	/* this statement backs up about 6 inches */
			while ((total_dist < reverse_dist) && (reverse == 1))
			{
				motor(MOTOR_JMPR, -100.0);		/* keep reversing */
				distance();
			}
			reverse = 0;
		}
		/* if (analog(IR_BK_LFT) > IR_THRESHOLD) an obstacle has been detected to the left, so turn right */
			/* try to have the sonar look right to warn of future obstacles */
		/* if (analog(IR_BK_RGHT) > IR_THRESHOLD) an obstacle has been detected to the right, so turn left */
			/* try to have the sonar look left to warn of future obstacles */
		/* if both the above are detected, stop, or slow, the motor and confirm with sonar */
			/* confirm(0) */
			/* once confirmed move forward */
			/* straighten();
			motor(MOTOR_JMPR, 100.0);		*/
	}
}

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 confirm(int direction)		/* check to make sure obstacles are where ir detects them */
{
	/* direction is 1 for forward and 0 for reverse */
	/* average_angle(&avg_angle); */
	/* based on value of avg_angle calculate where to look for objects */
}

void dir_angle()
{
	average_sensor(COMPASS1, &output1);	/* cos curve */
	average_sensor(COMPASS2, &output2);	/* sin curve */

	angle_res = 1.915;				/* this changes with upper_line and lower_line */

	if ((output1 >= upper_line) && (output2 >= upper_line))
	{
		angle = 45;
	}
	else if ((output1 <= lower_line) && (output2 <= lower_line))
	{
		angle = 225;
	}
	else if (output2 >= upper_line) 
	{
		/* output1 measures from 45 to 135 degrees */
		range1 = (upper_line - output1);
		angle_resolution = ((float)range1 * angle_res);
		angle = ((int)angle_resolution + 45);  
	}
	else if (output1 <= lower_line)
	{
		/* output2 measures from 135 to 225 degrees */
		range2 = (upper_line - output2);
		angle_resolution = ((float)range2 * angle_res);
		angle = ((int)angle_resolution + 135);
	}
	else if (output2 <= lower_line)
	{
		/* output1 measures from 225 to 315 degrees */
		range1 = (output1 - lower_line);
		angle_resolution = ((float)range1 * angle_res);
		angle = ((int)angle_resolution + 225);
	}
	else if (output1 >= upper_line)
	{
		/* output2 measures from 315 to 45 degrees */
		range2 = (output2 - lower_line);
		angle_resolution = ((float)range2 * angle_res);
		angle = ((int)angle_resolution + 315);
	}
	else 
	{
		angle = angle;
	}
	if (angle >= 360)
	{
		angle = angle - 360;		/* keep the values of angle between 0 and 360 */
	}
	/* the write functions are used to observe the outputs with hyperterminal */
/*	write("angle = ");
	write_int(angle);
	write("output1 = ");
	write_int(output1);
	write("output2 = ");
	write_int(output2); */
}

void distance()					/* measure the distance traveled */
{
	/* there are 6 counts per wheel revolution */
	if (analog(ODOMETER) < ODOM_THRESHOLD)
	{
		if (odom != 1)
		{
			total_dist++;					/* add to the count */
		}
		odom = 1;
		write("odom = ");
		write_int(odom);
		write_int(total_dist); 
	}
	else
	{
		odom = 0;
		write("odom = ");
		write_int(odom);
		write_int(total_dist); 
	}
	/* divide by 6 to get the number of inches traveled */
	/* if integers aren't accurate enough use float */
}

void follow_path()		/* recreates the recorded path */
{								/* receive() and object avoidance should still be running */
	int last_index;
	int follow_dist;
	int follow_dir;
	float follow_deg1;
/*	write("Entering follow_path()\n\r"); */
	last_index = path_index;
	path_index = 0;
	rst = 0;

	while ((path_index < (last_index - 1)) && (follow == 1))
	{
		follow_dist = dist[path_index + 1] - dist[path_index];
		follow_dir = dir[path_index + 1] - dir[path_index];
		follow_deg1 = deg[path_index + 1] - deg[path_index];
/*		write("follow_dist = ");
		write_int(follow_dist); */
		if ((follow_dir > -5) || (follow_dir < 5))
		{
			follow_dir = 0;
		}
/*		write("follow_dir = ");
		write_int(follow_dir); */
		move_fwd(follow_dist, follow_dir, follow_deg1);	

		path_index++;
	}
	path_index++;
	follow = 0;
	motor(MOTOR_JMPR, 0.0);			/* stop when finished following the path */
	straighten();
}

void initialize()
{
	init_servos(); 
	init_serial();						/* initialize the serial port */
	motor(MOTOR_JMPR, 0.0);			/* stop motor */
	total_dist = 0;					/* clear the distance counter */ 
	odom = 0;							/* set it so distance() triggers on a rising edge */
	rst = 1;								/* tells arbitrate the board has just been reset */
	straight_lvl = 0;					/* this makes it so the first location is recorded */
	follow_active = 0;
	average_angle(&avg_angle);		/* returns the current angle */
											/* scan the surroundings */
	poke(0x7000, 0xFF);				/* turn on the infared LED's */
	detect = 0;
	step = 0;
}

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

/* controls how far the robot moves based on the wheel encoder */
/* a travel_dist of 36 is 51 inches, 14 is 1.5 ft */
/* this seemed to be repeatable with a 1 inch accuracy */
/* turns until the robot reaches the specified angle change */
void move_fwd(int travel_dist, int angle_chg, float angle_deg1)		
{
	int end_dist;

/*	write("Entering move_fwd()\n\r"); */
	distance();
	end_dist = total_dist + travel_dist;

		/* call avoid(); to check for objects */
		/* or have arbitrate determine whether to exit the function and call avoid() */
		/* if (obj_fwd_left or obj_fwd_right = 1) record current distance */
			/* avoid_dist = total_dist; to save the state when robot veers of the path */
		/* check to see if any objects in the grid are too close */

	/* monitor compass to see when I reach the desired angle */
	j = DEG1_CHG;

	/* calculate the current angle
	average_angle(&avg_angle);			/* returns avg_angle */

	/* calculate what the new angle should be */
	new_angle = avg_angle + angle_chg;

	/* keep the value between 0 and 359 */
	if (new_angle >= 360)
	{
		new_angle = new_angle - 360;
	}
	if (new_angle < 0)
	{
		new_angle = new_angle + 360;
	}

	/* make sure Tracker turns in the most efficient direction */
	if (angle_chg > 180)
	{
		angle_chg = angle_chg - 360;
	}
	if (angle_chg < -180)
	{
		angle_chg = angle_chg + 360;
	}

	if (new_angle > avg_angle)
	{
		if (angle_chg >= 0)
		{
	 		while ((avg_angle < new_angle) && (total_dist < end_dist))
			{
/*				avoid();
				if (detect == 0)
				{ */
					turn(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, 100.0);			/* move robot forward */
/*				}
				else {break;} */
				distance(); 
			}
		}
		else
		{
			while ((avg_angle > 0) && (total_dist < end_dist))
			{	
/*				avoid();
				if (detect == 0)
				{ */
					turn(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, 100.0);			/* move robot forward */
/*				}
				else {break;} */
				distance(); 
			}
			while ((avg_angle > new_angle) && (total_dist < end_dist))
			{
/*				avoid();
				if (detect == 0)
				{ */
					turn(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, 100.0);			/* move robot forward */
/*				}
				else {break;} */
				distance(); 
			}
		}
	}
	else if (new_angle < avg_angle)
	{
		if (angle_chg < 0)
		{
			while ((new_angle < avg_angle) && (total_dist < end_dist))
			{
/*				avoid();
				if (detect == 0)
				{ */
					turn(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, 100.0);			/* move robot forward */
/*				}
				else {break;} */
				distance(); 
			}
		}
		else
		{
			while ((avg_angle < 360) && (total_dist < end_dist))
			{
/*				avoid();
				if (detect == 0)
				{ */
					turn(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, 100.0);			/* move robot forward */
/*				}
				else {break;} */
				distance(); 
			}
			while ((avg_angle < new_angle) && (total_dist < end_dist))
			{
/*				avoid();
				if (detect == 0)
				{ */
					turn(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, 100.0);			/* move robot forward */
/*				}
				else {break;} */
				distance(); 
			}
		}
	}
	else 
	{
		servo_deg1(deg1);							/* keep going in the same direction */
		while (total_dist < end_dist)
		{
/*				avoid();
				if (detect == 0)
				{ */
					motor(MOTOR_JMPR, 100.0);			/* move robot forward */
/*				}
				else {break;} */
				distance(); 
		}
	}
}

void obstacle_path()		/* keeps track of distances and angles traveled when avoiding obstacles */
{
	/* should be similar to record_path() */
	/* this function or another one will then guide the robot back onto the original path */
}

void receive()		/* receives characters from the serial port */
{
	char data;
	forward = 0;
	reverse = 0;
	turn_right = 0;	
	turn_left = 0;		
	record = 0;
	straight = 0; 

	while (1)
	{
		data = get_char();
		if ((data == 'B') || (data == 'b'))		/* apply the brakes */
		{
			motor(MOTOR_JMPR, 0.0); 
			/* set every other serial port variable, except record, to 0 */
				forward = 0;
				reverse = 0;
				turn_right = 0;	
				turn_left = 0;		
				straight = 0;
				follow = 0;
		}
		else if (((data == 'F') || (data == 'f')) && (record == 0))
		{
/*			write("follow = 1, rst = 0\n\r"); */
			follow = 1;
			rst = 0;					/* make sure the path is not followed until the board is reset */
/*			write("Spawning follow_path()\n\r"); */
			follow_pid = start_process(follow_path()); 
			follow_active = 1;
		}
		else if ((data == 'R') || (data == 'r'))		/* toggle record_mode on and off */
		{
			if (record == 1)
			{
				record = 0;		/* turn off record mode */
				kill_process(record_pid); 
/*				write("Killing record_mode()\n\r"); */
				sample();		/* sample the last data point */
				/* stop the robot when exiting record mode */
					forward = 0;
					reverse = 0;
					turn_right = 0;	
					turn_left = 0;		
					straight = 0;
					follow = 0;
					motor(MOTOR_JMPR, 0.0);
/*					write("Recorded data\n\r"); 
					for (i = 0; i < path_index; i++)
					{ 
						write_int(dist[i]);
						write_int(dir[i]);
					} */
			}
			else
			{
				record = 1;		/* put in record mode */
				/* stop and straighten the robot when entering record mode */	
					forward = 0;
					reverse = 0;
					turn_right = 0;	
					turn_left = 0;		
					straight = 1;
					follow = 0;
					motor(MOTOR_JMPR, 0.0);
					straighten();
				record_pid = start_process(record_mode()); 
			}
		}
/*		else if (data == '&') */
		else if ((data == 'I') || (data == 'i'))
		{
			if (reverse == 1)
			{
				motor(MOTOR_JMPR, 0.0);			
				reverse = 0;
				follow = 0;
			}
			else
			{
				forward = 1;	/* go forward */
				reverse = 0;	/* turn off reverse */
				motor(MOTOR_JMPR, 100.0); 
				follow = 0;
			}
		}
/*		else if (data == '(') */
		else if ((data == 'K') || (data == 'k'))
		{
			if (forward == 1)
			{
				motor(MOTOR_JMPR, 0.0);			/* stop before reversing */
				forward = 0;
				follow = 0;
			}
			else
			{
				reverse = 1;	/* go in reverse */
				forward = 0;	/* turn off forward */
				motor(MOTOR_JMPR, -100.0); 
				follow = 0;
			}
		}
/*		else if (data == ''')	*/
		else if ((data == 'L') || (data == 'l'))
		{
			if (turn_left == 1)
			{
				turn_right = 0;	
				turn_left = 0;		
				straight = 1;
				servo_deg1(STRAIGHT);
				follow = 0;
			}
			else
			{
				turn_right = 1;	/* turn right */
				turn_left = 0;		
				straight = 0;
				servo_deg1(RIGHT);
				follow = 0;
			}
		}
/*		else if (data == '%') */
		else if ((data == 'J') || (data == 'j'))
		{
			if (turn_right == 1)
			{
				turn_right = 0;	
				turn_left = 0;		
				straight = 1;
				servo_deg1(STRAIGHT);
				follow = 0;
			}
			else
			{
				turn_left = 1;		/* turn left */
				turn_right = 0;	
				straight = 0;
				servo_deg1(LEFT); 
				follow = 0;
			}
		}
/*		else if ((data == 'S') || (data == 's'))
		{
			straight = 1;		
			turn_right = 0;
			turn_left = 0;
			straighten(); 
			follow = 0;
		} */
		else
		{
			/* continue as previously instructed */
/*				forward = forward;
				reverse = reverse;
				turn_right = turn_right;	
				turn_left = turn_left;		
				straight = straight;
				follow = follow;
				record = record; */
		}
		if ((follow == 0) && (follow_active == 1))
		{
/*			write("Killing follow_path()\n\r"); */
			kill_process(follow_pid); 
			follow_active = 0;				
		}

/*		if ((follow == 1) && (rst == 1))	*/		/* wait for the board to be reset to begin */
/*		{
			write("Spawning follow_path()\n\r");
			follow_pid = start_process(follow_path());
			follow_active = 1;
		} */
	}
}

void record_mode()		/* control the robot through the serial port */
{
/*	write("Entering record_mode()\n\r"); */
	/* watch out for conflicts between user control and autonomous mode */
	/* add an led that lights up when in record mode */
	total_dist = 0;						/* clear the distance counter */ 
	odom = 0;								/* set it so distance() triggers on a rising edge */
	rst = 0;									/* a reset must occur before follow_path() */
	average_angle(&avg_angle);			/* keeps the angle updated for record_path() */
	distance();								/* keeps the distance traveled updated */
	last_angle = avg_angle;
	path_index = 0;

	while (1)
	{
		record_path();							/* keeps track of the path */
		average_angle(&avg_angle);			/* keeps the angle updated for record_path() */
		distance();								/* keeps the distance traveled updated */
	}
}

void record_path()		/* keeps track of distances and angles traveled when being programmed */
{								/* does not control the robots actual motion */
	/* take a sample when deg1 changes from straight */ 
	if ((straight_lvl == 1) && (straight == 0))
	{
		sample();
		straight_lvl = 0;
	}
	if ((straight_lvl == 0) && (straight == 1))	
	{
		sample();
		straight_lvl = 1;
	} 
	/* currently only record while going forward */
	/* record any changes in avg_angle of greater than 9 degrees, this will remove small imperfections in steering. */
	if ((avg_angle > (last_angle + 9)) || (avg_angle < (last_angle - 9)))
	{
		sample();
	}
}

void sample()		/* add the current avg_angle and total_dist values to their arrays */
{
/*	write("Entering sample()\n\r"); */
	dist[path_index] = total_dist;
	dir[path_index] = avg_angle;
	deg[path_index] = deg1;				/* record the current servo_deg1 setting */

	path_index++;
	last_angle = avg_angle;
}

void straighten()
{
	/* stop turning by smoothly straightening the wheels */
	/* implement logic to turn smoothly like turn() has */
	servo_deg1(STRAIGHT);
	deg1 = STRAIGHT;
}

void track()	/* tracks objects with sonar and adds them to the grid until confirm is called */
{

}

void turn(int angle_chg, float angle_deg1)		/* turns the robot and updates the angle */
{
	/* logic that determines turning smoothness, speed, and direction */

	if (angle_chg < 0) 						/* turn right */
	{
		if (deg1 < angle_deg1)				/* keeps robot from turning past RIGHT */
		{
			deg1 = deg1 + j; 
			servo_deg1(deg1); 
			j += DEG1_CHG;
		} 
	}
	else		/* turn left */
	{
		if (deg1 > angle_deg1)				/* keeps robot from turning past LEFT */
		{
			deg1 = deg1 - j; 
			servo_deg1(deg1); 
			j += DEG1_CHG;
		} 
	}
	average_angle(&avg_angle);				/* check the current angle */
}

void turn_back(int angle_chg, float angle_deg1)		/* turns the robot and updates the angle */
{
	/* logic that determines turning smoothness, speed, and direction */

	if (angle_chg > 0) 						/* turn right */
	{
		if (deg1 < angle_deg1)				/* keeps robot from turning past RIGHT */
		{
			deg1 = deg1 + j; 
			servo_deg1(deg1); 
			j += DEG1_CHG;
		} 
	}
	else		/* turn left */
	{
		if (deg1 > angle_deg1)				/* keeps robot from turning past LEFT */
		{
			deg1 = deg1 - j; 
			servo_deg1(deg1); 
			j += DEG1_CHG;
		} 
	}
	average_angle(&avg_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 Program Functions *************************************/

/*********************************** Testing Functions *****************************************/
void monitor_angle()
{
	while (1)
	{
		average_angle(&avg_angle);
		write_int(avg_angle); 
		wait(1000);
	}
}

void pattern()		/* Go in a pattern using the compass as a guide */
{
	move_fwd(9, 0, STRAIGHT);

	move_fwd(50, -30, RIGHT);

	straighten();
	wait(1000);

	move_fwd(50, 60, LEFT);

	straighten();
	wait(1000);

	move_fwd(50, -30, RIGHT);

	straighten();
	wait(1000);

	motor(MOTOR_JMPR, 0.0);
	wait(3000);

	motor(MOTOR_JMPR, -100.0);
	wait(1000);

	move_back(50, 30, RIGHT);

	straighten();
	wait(1000);

	move_back(50, -60, LEFT);

	straighten();
	wait(1000);

	move_back(50, 30, RIGHT);

	straighten();
	move_back(9, 0, STRAIGHT);

	motor(MOTOR_JMPR, 0.0);
}

void speed_test()			/* Tracker moves about 3.6 inches/second, varies with battery power */
{								/* this speed is not valid since I changed the gear ratio */
	straighten();
	motor(MOTOR_JMPR, 100.0);
	wait(5000);
}

void test_odometer()
{
	/* uncomment serial outputs in distance() to verify that the odometer is working */
	move_fwd(36, 0, 0.0);	/* moves Tracker in a straight line for 50 to 51 inches */
	motor(MOTOR_JMPR, 0.0);
}

void test_memory()			/* see if variable values are retained when a reset occurs */
{									/* if the variable is persistent it does not change on a reset */
	wait(2000);
	write_int(mem_test);
	if (mem_test == 71)
	{
		servo_deg1(RIGHT);
	}
	else
	{
		servo_deg1(LEFT);
	} 
	mem_test = 71;
	write_int(mem_test);
}

void test_serial()
{
	char tst;
	while (1)
	{
		tst = get_char();
		write("echo: ");
		put_char(tst);
		write("\n");
	}
}

void test_turning()
{
	move_fwd(28, 120, 25.0);		/* will turn 90 degrees before going 50 inches */
	straighten();
	wait(2000);
	motor(MOTOR_JMPR, 0.0);				/* stop robot */
}
/******************************** End of Testing Functions *************************************/

/* controls how far the robot moves based on the wheel encoder */
/* a travel_dist of 36 is 51 inches, 14 is 1.5 ft */
/* this seemed to be repeatable with a 1 inch accuracy */
/* turns until the robot reaches the specified angle change */
void move_back(int travel_dist, int angle_chg, float angle_deg1)		
{
	int end_dist;

/*	write("Entering move_back()\n\r"); */
	distance();
	end_dist = total_dist + travel_dist;

	/* monitor compass to see when I reach the desired angle */
	j = DEG1_CHG;

	/* calculate the current angle
	average_angle(&avg_angle);			/* returns avg_angle */

	/* calculate what the new angle should be */
	new_angle = avg_angle + angle_chg;

	/* keep the value between 0 and 359 */
	if (new_angle >= 360)
	{
		new_angle = new_angle - 360;
	}
	if (new_angle < 0)
	{
		new_angle = new_angle + 360;
	}

	/* make sure Tracker turns in the most efficient direction */
	if (angle_chg > 180)
	{
		angle_chg = angle_chg - 360;
	}
	if (angle_chg < -180)
	{
		angle_chg = angle_chg + 360;
	}

	if (new_angle > avg_angle)
	{
		if (angle_chg >= 0)
		{
	 		while ((avg_angle < new_angle) && (total_dist < end_dist))
			{
/*				avoid();
				if (detect == 0)
				{ */
					turn_back(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, -100.0);					/* move robot backwards */
/*				}
				else {break;} */
				distance(); 
			}
		}
		else
		{
			while ((avg_angle > 0) && (total_dist < end_dist))
			{	
/*				avoid();
				if (detect == 0)
				{ */
					turn_back(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, -100.0);					/* move robot backwards */
/*				}
				else {break;} */
				distance(); 
			}
			while ((avg_angle > new_angle) && (total_dist < end_dist))
			{
/*				avoid();
				if (detect == 0)
				{ */
					turn_back(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, -100.0);					/* move robot backwards */
/*				}
				else {break;} */
				distance(); 
			}
		}
	}
	else if (new_angle < avg_angle)
	{
		if (angle_chg < 0)
		{
			while ((new_angle < avg_angle) && (total_dist < end_dist))
			{
/*				avoid();
				if (detect == 0)
				{ */
					turn_back(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, -100.0);					/* move robot backwards */
/*				}
				else {break;} */
				distance(); 
			}
		}
		else
		{
			while ((avg_angle < 360) && (total_dist < end_dist))
			{
/*				avoid();
				if (detect == 0)
				{ */
					turn_back(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, -100.0);					/* move robot backwards */
/*				}
				else {break;} */
				distance(); 
			}
			while ((avg_angle < new_angle) && (total_dist < end_dist))
			{
/*				avoid();
				if (detect == 0)
				{ */
					turn_back(angle_chg, angle_deg1);		/* turn */
					motor(MOTOR_JMPR, -100.0);					/* move robot backwards */
/*				}
				else {break;} */
				distance(); 
			}
		}
	}
	else 
	{
		servo_deg1(deg1);							/* keep going in the same direction */
		while (total_dist < end_dist)
		{
/*				avoid();
				if (detect == 0)
				{ */
					motor(MOTOR_JMPR, -100.0);			/* move robot backwards */
/*				}
				else {break;} */
				distance(); 
		}
	}
}