/***********************************************************************************************
 * Title:			Control2.c
 *
 * Programmer:		Todd Martin
 *
 * Date:				April, 1999
 *
 * Version:			2
 *
 * Description:	Develop algorithms for recording the path.  Written in IC.
 *
 **********************************************************************************************/

/*************************************** Constants ********************************************/
int COMPASS1 = 0;				/* analog port number for the cos curve*/
int COMPASS2 = 2;				/* 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 = 100;		/* the value used in determining if an object has been detected */
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 */
/************************************ 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 total_dist;				/* keep track of the wheel encoder counts */
int total_dist_inches;		/* keep track of the wheel encoder counts in inches */
int old_dist_inches;
int chg_dist_inches;
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 = 0;
int reverse = 0;
int turn_right = 0;	
int turn_left = 0;		
int record = 0;
int straight = 0;
/************************************** End of Globals ****************************************/

/****************************************** Main **********************************************/
void main()
{
	initialize();
	/* start off going straight before turning */
	/* receive(); should run as a process */
	pattern();
	motor(MOTOR_JMPR, 0.0);		/* stop motor */
}
/*************************************** End of Main ******************************************/

/**************************************** Functions *******************************************/
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 < 10; i++) sum += analog(port);

	*result = sum / 10;
}

void average_angle(int *result)
{
	int sum = 0;
	int i;

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

	*result = sum / 5;
}

void avoid()		/* reads data from the forward IR and sonar and says if an obstacle is detected */
{
	/* if (analog(IR_FR_LFT) > IR_THRESHOLD) an obstacle has been detected to the left */
		/* return obj_fwd_left = 1; so 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 */
		/* return obj_fwd_right = 1; so robot should turn left */
		/* tell 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(1); */
		/* once confirmed call back_up() */
}

void back_up()		/* reads data from the back IR and sonar and tells the motors an obstacle is detected */
{
	/* while backing up */
		/* if (analog(IR_FWD_LFT) < IR_THRESHOLD) set variable fwd_clear */
		/* if (analog(IR_FWD_RGHT) < IR_THRESHOLD) set variable fwd_clear */
		/* if (fwd_clear = 1) continue in reverse for 6 inches or until an object is detected */
			/* distance(); */
			/* reverse_dist = total_dist_inches + 6; */
			/* while (total_dist_inches < reverse_dist) keep reversing and calling distance() */
		/* 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 */
}

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(); */
	/* based on value of 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;

	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 revolution */
	/* the pulse accumulator will overflow after approximately 22 feet */
	/* PAOVF in TFLG2 goes true after an overflow.  Set it to interrupt. */
/* pseudo code */
	/* get value of PACNT - $1027 */
		/* new_dist = PACNT; */
	/* check number of overflows and multiply by 256, then add this to the current PACNT */
		/* total_dist = (pa_ovrfl * 256) + new_dist; */
	/* divide by 6 to get the number of inches traveled. if integers aren't accurate enough use float */
		/* total_dist_inches = total_dist / 6; */
	/* if needed have it return total_dist and the distance from the last time the function was called */
		/* each program that calls distance() might keep track of this itself */
		/* chg_dist_inches = total_dist_inches - old_dist_inches; */
		/* old_dist_inches = total_dist_inches; */
}

void initialize()
{
	init_servos();
	motor(MOTOR_JMPR, 0.0);		/* stop motor */
	old_dist_inches = 0;			/* clear the distance counter */ 
										/* clear the pulse accumulator, PACNT - $1027 */
	average_angle();					/* returns the current angle */
										/* set the baud rate to 2400 so the wireless link will work */
										/* scan the surroundings */
	init_serial();					/* initialize the serial port */
}

void init_servos()
{
	servo_on();
	servo_deg1(STRAIGHT);		/* 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);
	}
}

void move_fwd(int travel_dist_inches)		/* controls how far the robot moves based on the wheel encoder */
{
	/* distance(); */
	/* start_dist_inches = total_dist_inches; */
	/* end_dist_inches = start_dist_inches + travel_dist_inches; */
	/* if (total_dist_inches < end_dist_inches) move robot forward */
	/* call avoid(); to check for objects */
	/* if (obj_fwd_left or obj_fwd_right = 1) call distance() */
		/* avoid_dist_inches = total_dist_inches; to save the state when robot veers of the path */
	/* check to see if any objects in the grid are too close */
}

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 pattern()		/* Go in a pattern using the compass as a guide */
{
	straighten();
	motor(MOTOR_JMPR, 100.0);
	wait(2000); 

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

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

	turn(-45); 
	dir_angle(); 
/*	average_angle(&avg_angle);
	write_int(avg_angle); */

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

void receive()		/* receives characters from the serial port */
{
	char data;
	data = get_char();
	if (data == 'B')		/* apply the brakes */
	{
		/* set every other serial port variable, except record, to 0 */
			forward = 0;
			reverse = 0;
			turn_right = 0;	
			turn_left = 0;		
			straight = 0;
	}
	else if (data == 'R')	/* checking for one character could be a problem if the buttons bounce */
	{
		if (record = 0)
		{
			record = 1;		/* put in record mode */
			/* stop the robot when entering record mode */
				forward = 0;
				reverse = 0;
				turn_right = 0;	
				turn_left = 0;		
				straight = 0;
			/* spawn process record_mode(); */
		}
		else
		{
			record = 0;		/* turn off record mode */
			/* kill process record_mode(); */
			/* stop the robot when exiting record mode */
				forward = 0;
				reverse = 0;
				turn_right = 0;	
				turn_left = 0;		
				straight = 0;
		}
	}
	else if (data == '&')
	{
		forward = 1;	/* go forward */
		reverse = 0;	/* turn off reverse */
	}
	else if (data == '(')
	{
		reverse = 1;	/* go in reverse */
		forward = 0;	/* turn off forward */
	}
	else if (data == ''')
	{
		turn_right = 1;	/* turn right */
		turn_left = 0;		
		straight = 0;
	}
	else if (data == '%')
	{
		turn_left = 1;		/* turn left */
		turn_right = 0;	
		straight = 0;
	}
	else if (data == 'S')
	{
		straight = 1;		/* go straight */
		turn_right = 0;
		turn_left = 0;
	}
}

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 */
	move();				/* monitors the serial port for forward or reverse */
	steer();				/* monitors the serial port for steering commands */
	record_path();		/* keeps track of the path */
		/* if record_path() is too processor intensive add a variable that will toggle it on and off */
}

void record_path()		/* keeps track of distances and angles traveled when being programmed */
{								/* does not control the robots actual motion */
	/* try using a minimum sample time of maybe 5 seconds. */
		/* or record any changes in angle of greater than 9 degrees */
	/* this will remove small imperfections in steering. */
	/* when turning try measuring the degrees turned and not worrying about the distance traveled */
	/* try saving the state of necessary variables onto a stack */
	/* how can I set up a stack specifically for this? */
		/* how would I allocate this stack space? */
		/* could I use a dynamic array? */
			/* average_angle(); */
			/* distance(); */
}

void steer()	/* turns when a character is detected through the serial port saying to turn */
{
	/* turns in small increments until told to stop */
	/* set the variable to 0 after performing the task */
	if (straight = 1)
	{
		straighten();
		straight = 0;
	}
	else if (turn_right = 1)
	{
		turn(-10);			/* turn right 10 degrees. hopefully this will end up being smooth */
								/* how can I make it so the user can turn more than 10 degrees at a time */
		turn_right = 0;
	}
	else if (turn_left = 1)
	{
		turn(10);			/* turn left 10 degrees. hopefully this will end up being smooth */
		turn_left = 0;
	}
}

void straighten()
{
	/* stop turning by smoothly straightening the wheels */
	/* how can I figure out what the current degree setting is? */
	/* 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 track()	/* tracks objects with sonar and adds them to the grid until confirm is called */
{

}

void turn(int angle_chg)	/* turns until robot reaches the specified angle change */
{
	/* monitor compass to see when I reach the desired angle */
	int new_angle;

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

	/* calculate what the new angle should be */
	new_angle = 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 > angle)
	{
		if (angle_chg >= 0)
		{
	 		while (angle < new_angle)
			{
				turn_direction(angle_chg);		/* turn */
			}
		}
		else
		{
			while (angle > 0)
			{
				turn_direction(angle_chg);		/* turn */
			}
			while (angle > new_angle)
			{
				turn_direction(angle_chg);		/* turn */
			}
		}
	}
	else if (new_angle < angle)
	{
		if (angle_chg < 0)
		{
			while (new_angle < angle)
			{
				turn_direction(angle_chg);		/* turn */
			}
		}
		else
		{
			while (angle < 360)
			{
				turn_direction(angle_chg);		/* turn */
			}
			while (angle < new_angle)
			{
				turn_direction(angle_chg);		/* turn */
			}
		}
	}
}

void turn_direction(int angle_chg)		/* turns the robot and updates the angle */
{
	/* logic that determines turning smoothness, speed, and direction */
	float j = 1.0;				/* used to turn in small increments */

	if (angle_chg < 0) 						/* turn right */
	{
/*		servo_deg1(STRAIGHT + j); */
/*		motor(MOTOR_JMPR, 100.0);	*/		/* motor must be moving to turn */
/*		if (j <= 20.0)					*/		/* keeps robot from turning past RIGHT */
/*		{
			j++;		
		} */
		servo_deg1(RIGHT);
		motor(MOTOR_JMPR, 100.0);
	}
	else		/* turn left */
	{
/*		servo_deg1(STRAIGHT - j); */
/*		motor(MOTOR_JMPR, 100.0);	*/		/* motor must be moving to turn */			
/*		if (j <= 15.0)					*/		/* keeps robot from turning past LEFT */
/*		{
			j++;
		} */
		servo_deg1(LEFT);
		motor(MOTOR_JMPR, 100.0);
	}
	average_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 ****************************************/