#include <tkbase.h>
#include <stdio.h>

#define Output_Latch *(unsigned char *)(0x7000)

void motor(int, int);
void LeftSonar(void);
void RightSonar(void);
void TurnAway(void);

long i, j;
int IrRt_tmp, IrLft_tmp, IrRt, IrLft, RtSpd, LftSpd;
int LeftThresh, RightThresh, counterlft, counterrt, SnrLvl;
int Rt = 0;
int Lft = 1;
int Rt_IR = 0;
int Lft_IR = 4;
int SonarLft = 0x08;
int SonarRt = 0x01;
int Sonar = 7;
int MDetect = 3;
int MetalThresh = 0x40;
int belt = 0x04;
int Servoval = 0;
int ServoDrive = 2300;
int ServoDig = 3600;
int Clear = 0;


 
 
 
 
void main(void)
{
	init_analog();
	init_motortk();
	init_clocktk();
	init_servos();
	init_serial();
	DDRD = 0xff;
	CLEAR_BIT(PORTD, belt);
	for(i = 3350; i > 1750; i--)
	servo(0, i);
	motor(0, 0);
	motor(1, 0);
	Output_Latch = 0x00;

 while(1)
 {
	LeftSonar();
	RightSonar();
	if ((RtSpd == -100) || (LftSpd == -100))
		TurnAway();
 	motor(Rt, RtSpd);
 	motor(Lft, LftSpd);
 	
 	if (analog(MDetect) > 240)
 		{
 		RtSpd = -100;
 		LftSpd = -100;
 		motor(Rt, RtSpd);
 		motor(Lft, LftSpd);
 		for(i = 0; i < 5000; i++)
 			{
 			for(j = 0; j < 1; j++);
 			}
 		motor(Rt, 0);
 		motor(Lft, 0);
 		SET_BIT(PORTD, belt);
 		for(i = ServoDrive; i < ServoDig; i++)
 			{
 			servo(Servoval, i);
 			for(j = 0; j < 5; j++);
 			}
 		motor(Rt, 25);
 		motor(Lft, 25); 
 		for(i = 0; i < 5000; i++)
 			{
 			for(j = 0; j < 5; j++);
 			}
 		motor(Rt, 0);
 		motor(Lft, 0);
 		for(i = ServoDig; i > ServoDrive; i--)
 			{
 			servo(Servoval, i);
 			for(j = 0; j < 5; j++);
 			}
 		CLEAR_BIT(PORTD, belt);
		}
 }
}
 
 
void motor(int index, int percent)
/**************************** motor *****************************
 * Description
 * Sets the speed and direction for the motors.
 * 
 * 
 *								
 *Usage: to controls motor direction
 ************************************************************************/
{	
	if (index == 0)
	{	/* Set the Direction for motor0*/
		if ( percent < 0)
		{	SET_BIT(PORTD, 0x20);		/* SET PD5 to high */
			printf("you are in the motor function index 0\n");
			motortk(index, percent);	/* Call motor command*/
		}		
		else 
		{	CLEAR_BIT(PORTD, 0x20);	/* Set PD5 to low */
			motortk(index, percent);	/* Call motor command*/
		}
		
	}
	else if (index == 1)
	{	/* Set the direction of the motor1 */
		printf("you are in the motor function index 1\n");
		if ( percent < 0)
		{	SET_BIT(PORTD, 0x10);		/* Set PD4 to high*/
			motortk(index, percent);	/* Call motor command*/
		}		
		else 
		{	CLEAR_BIT(PORTD, 0x10);	/* Set PD4 to low*/
			motortk(index, percent);	/* Call motor command*/
		}
		
	}
}
/**************************** motor **** *****************************/



void LeftSonar(void)
{
	counterlft = 0;
	SnrLvl = 255;
	Output_Latch = SonarLft;
	for(i = 0; i < 100; i++);
	Output_Latch = Clear;
	
	while((SnrLvl > 125) && (counterlft < 300))
	{	SnrLvl = analog(7);
		counterlft++;
	}
	if ((counterlft < 100) && (counterlft > 5))
		LftSpd = -100;
	else LftSpd = 100;
}


void RightSonar(void)
{
	counterrt = 0;
	SnrLvl = 255;
	Output_Latch = SonarRt;
	for(i = 0; i < 100; i++);
	Output_Latch = Clear;
	
	while((SnrLvl > 125) && (counterrt < 300))
	{	SnrLvl = analog(7);
		counterrt++;
	}
	if ((counterrt < 100) && (counterrt > 5))
		RtSpd = -100;
	else RtSpd = 100;
}

void TurnAway(void)
{
	
	motor(Rt, RtSpd);
 	motor(Lft, LftSpd);
 	
 	for(i = 0; i < 5000; i++);
 	
}