/***********************************************************************************************
 * Title:			Control4.c
 *
 * Programmer:		Todd Martin
 *
 * Date:				April, 1999
 *
 * Version:			4
 *
 * Description:	Develop algorithms for following the recorded the path.  Written in IC.
 *
 **********************************************************************************************/

/*************************************** 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 = 25.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 ****************************************/

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

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

/***************************************** 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;
persistent int follow;
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_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;
int odom;							/* keeps track of pulse levels for the odometer */
persistent int mem_test;		/* used in test_memory() */
int obj_fwd_left;
int obj_fwd_right;
int reverse_dist;
/************************************** End of Globals ****************************************/

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

/************************************ Program Functions ***************************************/
void arbitrate()								/* controls what process or function is in charge */
{
	while (1)
	{
		if (follow == 1)						/* follow takes precedence over record */
		{
			if (rst == 1)						/* wait for the board to be reset to begin */
			{
				follow_path();
			}
		}
		else if (record == 1)
		{
			average_angle(&avg_angle);		/*	check the current direction */
			move();								/* controls the motor */	
			steer();								/* controls the steering servo */		
			record_path();						/* record the path */
		}
		else
		{
			move();								/* controls the motor */	
			steer();								/* controls the steering servo */		
		}
	}
}

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;
	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 */
		/* confirm(1); */
		/* once confirmed stop and call back_up() */
		motor(MOTOR_JMPR, 0.0);			/* stop before backing up */
		reverse = 1;
		back_up();
	}
/*	if (obj_fwd_left == 1)
	{
		servo_deg1(RIGHT);
	}
	if (obj_fwd_right == 1)
	{
		servo_deg1(LEFT);
	} */
}

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_FWD_LFT) < IR_THRESHOLD) && (analog(IR_FWD_RGHT) < IR_THRESHOLD)) 
		{
			fwd_clear = 1;
		}
		if (fwd_clear == 1) */	/* continue in reverse for 6 inches or until an object is detected */
			distance(); 
			reverse_dist = total_dist + 9; 	/* 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 */
	}
	motor(MOTOR_JMPR, 0.0);		/* stop the motor to show back_up() is finished */
}

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;
	last_index = path_index;
	path_index = 0;
	rst = 0;

	while ((path_index < last_index) && (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];
		move_fwd(follow_dist, follow_dir, follow_deg1);	

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

void initialize()
{
	init_servos();
	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 */
	average_angle(&avg_angle);		/* returns the current angle */
											/* scan the surroundings */
	poke(0x7000, 0xFF);				/* turn on the infared LED's */
	init_serial();						/* initialize the serial port */
}

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

void move()		/* while in record mode monitor the forward and reverse variables */
{
	/* maybe I could have it so hitting reverse after going forward will stop the robot */
	/* hitting reverse again will then make the robot go in reverse */
	if (forward == 1)
	{
		motor(MOTOR_JMPR, 100.0);
	}
	else if (reverse == 1)
	{
		motor(MOTOR_JMPR, -100.0);
	}
	else
	{
		motor(MOTOR_JMPR, 0.0);
	}
}

/* controls how far the robot moves based on the wheel encoder */
/* a travel_dist of 36 is 4 ft, 18 is 2 ft, 9 is 11 inches */
/* 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;

	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))
			{
				turn(angle_chg, angle_deg1);		/* turn */
				motor(MOTOR_JMPR, 100.0);			/* move robot forward */
				distance(); 
			}
		}
		else
		{
			while ((avg_angle > 0) && (total_dist < end_dist))
			{	
				turn(angle_chg, angle_deg1);		/* turn */
				motor(MOTOR_JMPR, 100.0);			/* move robot forward */
				distance(); 
			}
			while ((avg_angle > new_angle) && (total_dist < end_dist))
			{
				turn(angle_chg, angle_deg1);		/* turn */
				motor(MOTOR_JMPR, 100.0);			/* move robot forward */
				distance(); 
			}
		}
	}
	else if (new_angle < avg_angle)
	{
		if (angle_chg < 0)
		{
			while ((new_angle < avg_angle) && (total_dist < end_dist))
			{
				turn(angle_chg, angle_deg1);		/* turn */
				motor(MOTOR_JMPR, 100.0);			/* move robot forward */
				distance(); 
			}
		}
		else
		{
			while ((avg_angle < 360) && (total_dist < end_dist))
			{
				turn(angle_chg, angle_deg1);		/* turn */
				motor(MOTOR_JMPR, 100.0);			/* move robot forward */
				distance(); 
			}
			while ((avg_angle < new_angle) && (total_dist < end_dist))
			{
				turn(angle_chg, angle_deg1);		/* turn */
				motor(MOTOR_JMPR, 100.0);			/* move robot forward */
				distance(); 
			}
		}
	}
	else 
	{
		servo_deg1(deg1);							/* keep going in the same direction */
		while (total_dist < end_dist)
		{
			motor(MOTOR_JMPR, 100.0);			/* move robot forward */
			distance(); 
		}
		motor(MOTOR_JMPR, 0.0);					/* stop moving forward */
	}
}

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;
	int i;
	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))
		{
			follow = 1;
			rst = 0;					/* make sure the path is not followed until the board is reset */
		}
		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);
				dist[path_index] = total_dist;			/* record the last data points */
				dir[path_index] = avg_angle;
				deg[path_index] = deg1;
				/* 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("\n\rtesting 1 2 3\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 == '&')
		{
			if (reverse == 1)
			{
				motor(MOTOR_JMPR, 0.0);			/* stop before reversing */
				reverse = 0;
				follow = 0;
			}
			else
			{
				forward = 1;	/* go forward */
				reverse = 0;	/* turn off reverse */
				motor(MOTOR_JMPR, 100.0); 
				follow = 0;
			}
		}
		else if ((data == 'G') || (data == 'g'))
		{
			if (reverse == 1)
			{
				motor(MOTOR_JMPR, 0.0);			/* stop before reversing */
				reverse = 0;
				follow = 0;
			}
			else
			{
				forward = 1;	/* go forward */
				reverse = 0;	/* turn off reverse */
				motor(MOTOR_JMPR, 100.0); 
				follow = 0;
			}
		}
		else if (data == '(')
		{
			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 == ''')	
		{
			turn_right = 1;	/* turn right */
			turn_left = 0;		
			straight = 0;
			servo_deg1(RIGHT);
			follow = 0;
		}
		else if (data == '%')
		{
			turn_left = 1;		/* turn left */
			turn_right = 0;	
			straight = 0;
			servo_deg1(LEFT); 
			follow = 0;
		}
		else if ((data == 'S') || (data == 's'))
		{
			straight = 1;		/* go straight */
			turn_right = 0;
			turn_left = 0;
			straighten(); 
			follow = 0;
		}
		if ((follow == 0) && (follow_pid != 99))
		{
			kill_process(follow_pid);
			follow_pid = 99;					/* I chose 99 because I won't have that many processes running */
		}
		if ((follow == 1) && (rst == 1))			/* wait for the board to be reset to begin */
		{
			follow_pid = start_process(follow_path());
		}
	}
}

void record_mode()		/* control the robot through the serial port */
{
	/* 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() */
	last_angle = avg_angle;
	dist[0] = total_dist;
	dir[0] = last_angle;
	deg[0] = deg1;
	path_index = 1;						/* keeps track of the next array location */

	while (1)
	{
		average_angle(&avg_angle);			/* keeps the angle updated for record_path() */
		distance();								/* keeps the distance traveled updated */
/*		start_process(move());	*/			/* monitors the serial port for forward or reverse */
/*		start_process(steer());	*/			/* monitors the serial port for steering commands */
		record_path();							/* keeps track of the path */
	}
}

void record_path()		/* keeps track of distances and angles traveled when being programmed */
{								/* does not control the robots actual motion */
	/* 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)))
	{
		/* add the current avg_angle and total_dist values to their arrays */
		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 steer()	/* turns when a character is detected through the serial port saying to turn */
{
	/* turns in small increments until told to stop */
	/* needs to limit the maximum amount the wheels turn so they don't get knocked out of alignment */
	/* set the variable to 0 after performing the task */
	if (turn_right == 1)
	{
		deg1 += 10.0;
		servo_deg1(deg1); 
		turn_right = 0;
	}
	else if (straight == 1)
	{
		write("straight == 1");
		straighten(); 
		straight = 0;
	}
	else if (turn_left == 1)
	{
		deg1 -= 5.0;
		servo_deg1(deg1);
		turn_left = 0;
	}
}

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 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 */
{
	straighten();
	motor(MOTOR_JMPR, 100.0);
	wait(2000); 

/*	dir_angle(); 
	average_angle(&avg_angle);
	write_int(avg_angle); */

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

/*	dir_angle(); 
	average_angle(&avg_angle);
	write_int(avg_angle); */

	straighten();
	motor(MOTOR_JMPR, 100.0);
	wait(2000); 
}

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()
{
	motor(MOTOR_JMPR, 100.0);
	move_fwd(36, 0, 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");
	}
}
/******************************** End of Testing Functions *************************************/