/***********************************************************************************************
 * 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 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 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 i;						/* a counter for loops */
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;
/************************************** End of Globals ****************************************/

/****************************************** Main **********************************************/
void main()
{
	initialize();
	pattern();
	motor(MOTOR_JMPR, 0.0);		/* stop motor */
}
/*************************************** End of Main ******************************************/

/**************************************** Functions *******************************************/
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 reverse() */
}

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 */
	/* dir_angle(); */
	/* based on value of angle calculate where to look for objects */
}

void dir_angle()
{
	output1 = analog(COMPASS1);	/* cos curve */
	output2 = analog(COMPASS2);	/* sin curve */

	angle_res = 1.915;

	if (output2 > upper_line) 
	{
		/* output1 measures from 45 to 135 degrees */
		range1 = (output1 - lower_line);
		angle_resolution = ((float)range1 * angle_res);
		angle = ((int)angle_resolution + 45);  
	}
	else if (output1 < lower_line)
	{
		/* output2 measures from 135 to 225 degrees */
		range2 = (output2 - lower_line);
		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 if ((output1 >= upper_line) && (output2 >= upper_line))
	{
		angle = 45;
	}
	else if ((output1 <= upper_line) && (output2 <= upper_line))
	{
		angle = 225;
	}
	else
	{
		angle = angle;
	}
	if (angle >= 360)
	{
		angle = angle - 360;		/* keep the values of angle between 0 and 360 */
	}
	write_int(angle);
}

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 */
	dir_angle();					/* returns the current angle */
										/* set the baud rate to 2400 so the wireless link will work */
										/* scan the surroundings */
	init_serial();
}

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

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

/*	turn(45); */
	dir_angle();

	straighten();
/*	motor(MOTOR_JMPR, 100.0); */
	wait(1000);

/*	turn(-45); */
	dir_angle();

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

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. */
	/* 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? */
	/* could I use a dynamic array? */
		/* dir_angle(); */
		/* distance(); */
}

void reverse()		/* 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 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 track()	/* tracks objects with sonar and adds them to the grid until confirm is called */
{

}

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

	/* calculate the current angle
	dir_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 amount */
	if (angle_chg < 0) 		/* turn right */
	{
		servo_deg1(RIGHT);
		motor(MOTOR_JMPR, 100.0);			/* run motor at half speed */
	}
	else		/* turn left */
	{
		servo_deg1(LEFT);
		motor(MOTOR_JMPR, 100.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 ****************************************/