/***********************************************************************************************
 * Title:			Control3.c
 *
 * Programmer:		Todd Martin
 *
 * Date:				April, 1999
 *
 * Version:			3
 *
 * Description:	Develop algorithms for recording 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 = 110;		/* the value used in determining if an object has been detected */
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_direction */
/************************************ 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 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 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 dist[50];					/* allocate space for record path */
int dir[50];					/* allocate space for record path */
float deg[50];					/* allocate space for record path */
int path_index;
/************************************** End of Globals ****************************************/

/****************************************** Main **********************************************/
void main()
{
	initialize();
	/* start off going straight before turning */
/*	start_process(receive()); 	*/		/* look for data entering the serial port */
/*	start_process(distance()); */
	move_fwd(36);
/*	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 < 3; i++) sum += analog(port);

	*result = sum / 3;
}

void average_angle(int *result)
{
	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 */
{
	/* 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 + 36; this statement backs up 6 inches */
			/* while (total_dist < reverse_dist) keep reversing */
		/* 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(&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) > IR_THRESHOLD)
	{
		total_dist++;					/* add to the count */
	}
	/* 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;

	while (path_index < last_index)
	{
		follow_dist = dist[path_index + 1] - dist[path_index];
		start_process(move_fwd(follow_dist));		/* move_fwd() and turn() must be running at the same time */

		follow_dir = dir[path_index + 1] - dir[path_index];
		follow_deg1 = deg[path_index + 1] - deg[path_index];
		start_process(turn(follow_dir, follow_deg1));

		/* wait for turn() and move_fwd() to finish. might be able to use a signal placed at the end of the functions */

		path_index++;
	}
}

void initialize()
{
	init_servos();
	motor(MOTOR_JMPR, 0.0);			/* stop motor */
	total_dist = 0;					/* clear the distance counter */ 
	average_angle(&avg_angle);		/* returns the current angle */
											/* scan the surroundings */
	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 */
{
	while (1)
	{
		/* 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 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;
	forward = 0;
	reverse = 0;
	turn_right = 0;	
	turn_left = 0;		
	record = 0;
	straight = 0;

	while (1)
	{
		data = get_char();
		if ((data == 'F') || (data == 'f'))
		{
			forward = 1;	/* go forward */
			reverse = 0;	/* turn off reverse */
motor(MOTOR_JMPR, 100.0);
		}
		else 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;
			/* kill turn() with the pid number - turn_pid, if turn() needs to be run as a standalone process */
		}
		else if ((data == 'R') || (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;
				record_pid = start_process(record_mode());		/* spawn process record_mode(); */
			}
			else
			{
				record = 0;		/* turn off record mode */
				kill_process(record_pid);		/* kill process record_mode(); */
				/* I will also need to kill the processes that record_mode() spawned */
				/* stop the robot when exiting record mode */
					motor(MOTOR_JMPR, 0.0);
					forward = 0;
					reverse = 0;
					turn_right = 0;	
					turn_left = 0;		
					straight = 0;
			}
		}
		else if (data == '&')
		{
			forward = 1;	/* go forward */
			reverse = 0;	/* turn off reverse */
motor(MOTOR_JMPR, 100.0);
		}
		else if (data == '(')
		{
			reverse = 1;	/* go in reverse */
			forward = 0;	/* turn off forward */
motor(MOTOR_JMPR, -100.0);
		}
		else if ((data == 'T') || (data == 't'))
		{
			turn_right = 1;	/* turn right */
			turn_left = 0;		
			straight = 0;
servo_deg1(RIGHT);			
		}
		else if (data == ''')	
		{
			turn_right = 1;	/* turn right */
			turn_left = 0;		
			straight = 0;
servo_deg1(RIGHT);
		}
		else if (data == '%')
		{
			turn_left = 1;		/* turn left */
			turn_right = 0;	
			straight = 0;
servo_deg1(LEFT);
		}
		else if ((data == 'S') || (data == 's'))
		{
			straight = 1;		/* go straight */
			turn_right = 0;
			turn_left = 0;
straighten();
		}
	}
}

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 */
	start_process(average_angle(&avg_angle));		/* keeps the angle updated for record_path() */
	start_process(move());				/* monitors the serial port for forward or reverse */
	start_process(steer());				/* monitors the serial port for steering commands */
	start_process(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 */
	path_index = 0;				/* keeps track of the next array location */
	last_angle = avg_angle;
	dist[path_index] = total_dist;
	dir[path_index] = last_angle;

	/* currently only record while going forward */
	while (1)
	{
		/* 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 */
{
	while (1)
	{
		/* 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 */
			deg1 += 5.0;
			servo_deg1(deg1); 
			turn_right = 0;
		}
		else if (turn_left = 1)
		{
	/*		turn(10);	*/		/* turn left 10 degrees. hopefully this will end up being smooth */
			deg1 -= 5.0;
			servo_deg1(deg1);
			turn_left = 0;
		}
	}
}

void straighten()
{
	/* stop turning by smoothly straightening the wheels */
	/* follows turn() */
	/* implement logic to turn smoothly like turn_direction() has */
	servo_deg1(STRAIGHT);
	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 move_fwd(int travel_dist)		/* controls how far the robot moves based on the wheel encoder */
{
	int travel_dist;
	int end_dist;

	end_dist = total_dist + travel_dist;

/* try saying while (total_dist < end_dist) and turn parameter, then use break to exit the loop */
	while (total_dist < end_dist) 				
	{
		motor(MOTOR_JMPR, 100.0);				/* move robot forward */
		distance();

		/* call avoid(); to check for objects */
		/* 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 */
	}
	motor(MOTOR_JMPR, 0.0);						/* stop moving forward */
}

void turn(int angle_chg, float angle_deg1)	/* turns until robot reaches the specified angle change */
{
	/* monitor compass to see when I reach the desired angle */
	int new_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)
			{
				turn_direction(angle_chg, angle_deg1);		/* turn */
			}
		}
		else
		{
			while (avg_angle > 0)
			{
				turn_direction(angle_chg, angle_deg1);		/* turn */
			}
			while (avg_angle > new_angle)
			{
				turn_direction(angle_chg, angle_deg1);		/* turn */
			}
		}
	}
	else if (new_angle < avg_angle)
	{
		if (angle_chg < 0)
		{
			while (new_angle < avg_angle)
			{
				turn_direction(angle_chg, angle_deg1);		/* turn */
			}
		}
		else
		{
			while (avg_angle < 360)
			{
				turn_direction(angle_chg, angle_deg1);		/* turn */
			}
			while (avg_angle < new_angle)
			{
				turn_direction(angle_chg, angle_deg1);		/* turn */
			}
		}
	}
	else
	{
		servo_deg1(deg1);						/* keep going in the same direction */
	}
}

void turn_direction(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); 
			motor(MOTOR_JMPR, 100.0);		/* motor must be moving to turn */
			j += DEG1_CHG;
		} 
	}
	else		/* turn left */
	{
		if (deg1 > angle_deg1)				/* keeps robot from turning past LEFT */
		{
			deg1 = deg1 - j; 
			servo_deg1(deg1); 
			motor(MOTOR_JMPR, 100.0);		/* motor must be moving to turn */			
			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 Functions ****************************************/