/************************************************************************
 * Title        rockofin.c                                	                
 * Programmer	Darren Kelley	        				
 * Date         April 15, 1999      	                                
 * Version	3			                		
 *								        
 * Description
 *   This program allows my robot (rear wheel drive and front wheel steering
 *	 to perform obstacle avoidance, scan for the brightest area in the sky
 *   and launch a rocket(PD3).FOR FRONT SENSORS AT INSIDE ANGLE OF 120 DEGREES
 ************************************************************************/
 
/**************************** Includes **********************************/

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

/************************ End of includes *******************************/

/**************************** Defines **********************************/

#define poke *(unsigned char *) (0x7000)
#define irrt analog(0)
#define irlf analog(1)
#define sonar40 analog(2)
#define cds_side analog(3)
#define sonar24 analog(4)
#define bend analog(5)
#define cds_box analog(6)
#define SnrTxL		0x02
#define SnrTxR		0x01
#define Clear		0xC0
#define bit3		0x08
#define bit4		0x10
#define bit5		0x20
#define sonar_max	24
#define sonar_thrhld	75
#define low_degree 1000
#define center_degree 2650
#define search_delay 2670
#define increment 5
#define pad_motor 	0
#define maxtime 200
/************************ End of Defines *******************************/

/**************************** Prototypes **********************************/
void main_init(void);
void adjust_speed(int, int);
void motor(int, int);
void straight(void);
void fwd_right(int);
void fwd_left(int);
void bk_right(int);
void bk_left(int);
void bk_straight(int);
void obstacle_avoid(void);
void start_search(void);
void search(void);
void sonar_rt(void);
void sonar_lf(void);
void search(void);
void start_pad(void);
void adjust_pad(void);
void launch(void);
void laun_seq(void);
/**************************End of Prototypes ******************************/

/**************************** Globals **********************************/
int main_cnt = 0;
int speed = 30;
int rev_speed = -30;
int center = 3000;
int right = 2700;
int left = 3300;
int ir_thrhldlf = 105;
int ir_thrhldrt = 105;
int ir_maxlf = 110;
int ir_maxrt = 110;
int present_speed0 = 0;
int present_speed1 = 0;
int speed_delay = 87;			/* delay = 174 for loop corresponds to 2 ms*/
int irert = 0x40;
int irelf = 0x80;
int sonarrt_cnt = 0;
int sonarlf_cnt = 0;
int up, count, j, high, found, high_count;/* globals for search start and launch*/
int degree, location, high_degree;
int gotit =0;
int launched = 0;
int movedelay = 30;
/**************************End of globals ******************************/

/**************************  Main   ************************************/
void main(void)
{
  main_init();
  while (irrt < ir_maxrt);	/* Wait until the right ir is set high to turn on*/
  while(1)
  {
  	for(main_cnt=0; main_cnt<maxtime; main_cnt++)
  	{
	obstacle_avoid();
/*	printf("The analog0 value is %d     The analog1 value is %d\n", irrt, irlf);*/
	}
	if(launched == 0)
	{
		motor(1, 0);
		laun_seq();
		launched = 1;
	}
  }	
}
/**************************** End Main  *********************************/

void main_init(void)
/**************************** initialization *****************************
 * Description
 * all initializations
 * 
 *									
 *Usage: 
 ************************************************************************/
{
	int i;
/* Initialize the analog, motors, servo, clock, and serial */
  init_analog();
  init_motortk();
  init_clocktk();
  init_servos();
  init_serial();
  DDRD = 0x38;		/* set Port D bit 3, 4, and 5 as output bits*/
  CLEAR_BIT(PORTD, bit3);		/*Clear port d bit 3*/
  poke = 0x00;		/* turn off all 40 kHz signals */
  for(i=0; i<100; i++);	/* wait for ir to shut off*/
  ir_thrhldrt = irrt + 5;	/* Initialize thresholds values */
  ir_thrhldlf = irlf + 5;	/* Assumes position where vehicle is reset*/
  ir_maxrt = irrt + 10;		/* is away from all obstacles*/
  ir_maxlf = irlf + 10;
  poke = 0xC0;		/* turn on right and left ir 40 kHz signals */
  motor(0,0);		/* Initialize motors to 0*/
  motor(1,0); 
}
/**************************** initializaion *****************************/
 
/**************************** obstacle_avoid *****************************
 * Description
 * Steers the vehicle in the appropriate direction depending on the ir
 * sensor and sonar sensor readings.
 *									
 *Usage: to control robots direction and speed.
 ************************************************************************/
void obstacle_avoid(void)
{
        int i, k;                  /* integer for a counter*/
		sonar_lf();					/*check left sonar. returns counter value*/
		sonar_rt();
	printf("Sonar left is %d and the Sonar right is %d\n", sonarrt_cnt, sonarlf_cnt);
/* check if the left side of the robot is at the minimum distance from an 
*  object.  if back up to the left.					*/
		if(irrt>ir_thrhldrt)
		{
			bk_right();
		}
		else if(irlf>ir_thrhldlf)
		{
			bk_left();
		}
		else if(sonarrt_cnt<sonar_max && sonarlf_cnt<sonar_max)
		{
                        	bk_left();
		}

		else if(sonarrt_cnt<sonar_max && sonarrt_cnt<sonarlf_cnt)
		{
                       bk_right();
		}

/* check if the left side of the robot is approaching an object.  if so 
*  turn to the right.							*/
		else if(sonarrt_cnt<sonar_thrhld && sonarrt_cnt<sonarlf_cnt)
		{
                    fwd_left();
		}
 
/* check if the right side of the robot is at the minimum distance from an 
*  object.  if back up to the right.					*/
		else if(sonarlf_cnt<sonar_max && sonarlf_cnt<sonarrt_cnt)
		{
                       bk_left();
		}

/* check if the right side of the robot is approaching an object.  if so 
*  turn to the left.							*/
		else if(sonarlf_cnt<sonar_thrhld && sonarlf_cnt<sonarrt_cnt)
		{
                      fwd_right();
		}

/* otherwise go straight at a constant speed			*/
    	else
    		{
    			straight();
    		}    
}
/**************************** obstacle_avoid *****************************/


void motor(int index, int percent)
/**************************** motor *************************************
 * Description
 * Sets the speed and direction for the motors.
 * 
 * 
 *								
 *Usage: to controls motor direction
 ************************************************************************/
{	
	printf("I'M in the motor function\n");
	if ( index == 0)
	{printf("index = 0\n");
		/* Set the Direction for motor0*/
		if ( percent < 0)
		{	SET_BIT(PORTD, 0x20);		/*For reverse index 0 SET PD5 to high */
			motortk(index, percent);	/* Call motor command*/
		}		
		else 
		{	CLEAR_BIT(PORTD, 0x20);	/* Set PD5 to low */
			motortk(index, percent);	/* Call motor command*/
		}
		
	}
	if (index == 1)
	{printf("index = 1\n");
		/* Set the direction of the motor1 */
		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 straight(void)	
/**************************** straight *****************************
 * Description
 * Sets the front servo to straight and motortk to motortk speed.
 * 
 *									
 *Usage: to control robots direction
 ************************************************************************/
{	printf("Going straight");
	motor(1, speed);
	servo(0, center);
}
/**************************** straight *****************************/

void fwd_right(void)
/**************************** fwd_right *****************************
 * Description
 * Sets the front servo to the right and motortk to motortk speed.
 * 
 *									
 *Usage: to control robots direction
 ************************************************************************/
{	
	printf("Going right");
	motor(1, speed);
	servo(0, right);
}
/**************************** fwd_right *****************************/

void fwd_left(void)
/**************************** fwd_left *****************************
 * Description
 * Sets the front servo to the left and motortk to motortk speed.
 * 
 *									
 *Usage: to control robots direction
 ************************************************************************/
{
		printf("Going left");
		motor(1, speed);
		servo(0, left);
}
/**************************** fwd_left *****************************/

void bk_right(void)
/**************************** bk_right *****************************
 * Description
 * Sets the front servo to the right in order to turn right backing up 
 * and motortk to the backwards direction with half motortk speed.
 *									
 *Usage: to control robots direction
 ************************************************************************/
{	
		printf("Backing up right");
		motor(1, rev_speed);
		servo(0, right);
}
/**************************** bk_right *****************************/

void bk_left(void)
/**************************** bk_left *****************************
 * Description
 * Sets the front servo to the left in order to turn left backing up 
 * and motortk to the backwards direction with half motortk speed.
 *									
 *Usage: to control robots direction
 ************************************************************************/
{
		printf("Backing up left");
		motor(1, rev_speed);
		servo(0, left);
}
/**************************** bk_left *****************************/

void bk_straight(void)
/**************************** bk_straight *****************************
 * Description
 * Sets the front servo to straight and motortk to motortk rev_speed.
 * 
 *									
 *Usage: to control robots direction
 ************************************************************************/
{	
		printf("Backing up straight");
		motor(1, rev_speed);
		servo(0, center);
}
/**************************** bk_straight *****************************/

void sonar_lf(void)
/**************************** sonar_lf *****************************
 * Description
 * pulses left sonar transmitter for 1ms checks the sonar receiver 
 * for a pulse
 *									
 *Usage: to control robots direction
 ************************************************************************/
{			int i;
			sonarlf_cnt = 0;
			poke = SnrTxL;
			for (i = 0; i < 100; i++);
			poke = Clear;
			for(i=0; i<100; i++);
			while((sonar40 > 200) && (sonarlf_cnt < 300))
			{
				sonarlf_cnt++;
			}
			if(sonarlf_cnt<=10)
			{
				sonarlf_cnt = 300;
			}
			
			printf("%d \n\n", sonarlf_cnt);
}
/**************************** sonar_lf *****************************/

void sonar_rt(void)
/**************************** sonar_rt *****************************
 * Description
 * pulses right sonar transmitter for 1ms checks the sonar receiver 
 * for a pulse
 *									
 *Usage: to control robots direction
 ************************************************************************/
 {			int i;
 			sonarrt_cnt = 0;		/* initialize counter to 0*/
			poke = SnrTxR;			/*turn sonar transducer on*/
			for (i = 0; i < 100; i++);	/* wait for 1 ms*/
			poke = Clear;				/*turn sonar transducer off*/
			for (i = 0; i<100; i++);	/*delay to reduce effects of transference of signal*/
			while((sonar40 > 200) && (sonarrt_cnt < 300))
			{
				sonarrt_cnt++;
			}
			if(sonarrt_cnt<=10)
			{
				sonarrt_cnt = 300;
			}
			printf("%d \n\n", sonarrt_cnt);
}
/*****************************sonar_rt*******************************


/*****************************start_pad************************
/ Start function moves pad form center point to full right 
/ starting postion to begin searching for brightest area.
/************************************************************/
void start_pad(void)
{
	degree = center_degree;		/* initialize degree to center position*/
	while(degree>low_degree)
	{
		degree = degree - increment;
		servo(1, degree);
        for(j=0; j<search_delay; j++);	/*delay loop to wait 30 ms*/
	}
}
/*****************************start_pad******************************/


/********** Search function  ****************************************
 * Description
 * searches for the highest value of light seen by the cds cell in the 3
 * sided box located on the arm attached to the launch pad
 *									
 *Usage: to control the launch pads direction
 ************************************************************************/
void search(void)
{ 		/* initialize variables*/
              up = 0;
              degree = 1000;
              high_degree = 1000;
              j=0;
              count=0;
              found = 0, high_count = 0;
              location=1000;
              high=255;
        while(gotit == 0)
        {
                if(degree<4400 && found == 0)
                {                 
                    if (cds_side<=high)
                    {
                       high=cds_side;
                       high_degree=degree;
                    }
                    degree = degree+increment;
                    location=degree;
                }
                else if (found == 0)
                {     
                	found=1;
                }
                if (found == 1 && degree>=high_degree) 
                {                 
					degree = degree - increment;                   
				}
				else if (found == 1 &&  degree<high_degree)
				{
					gotit = 1;
				}
                else              
                {
                        degree=degree;                	
                }
                servo(1, degree);
                for(j=0; j<search_delay; j++);	/*delay loop to wait 30 ms*/
        }
}

/********************** search ************************************

/ ******************* adjust_pad **********************************/
void adjust_pad(void)
{
	int found =0;
	int down = 1;
	int gotit = 0;
	int high = 255;
	int angle = 120;	/* variable used to store angle of highest light value*/
	while(ready == 0)
	{
		if((bend<135) && (down == 1))
		{
			motor(pad_motor, -100);
			printf("Motor going down");
			if ( cds_box < high)
			{
				high = cds_box;
				angle = bend;
			}
		}
		else if (bend>angle)
		{
			motor(pad_motor, 100);
			printf("Motor going up");
			down = 0;
		}
		else
		{
			motor(pad_motor, 0);
			printf("Done !!");
			found = 1;
		}
	}	
}
/*********************************** adjust_pad *********************/

void launch(void)
/**************************** launch *****************************
 * Description
 * sets port d bit 3 to high for 10 ms causing a fuse to ignite and launch
 * the rocket
 * 
 *								
 *Usage:
 ************************************************************************/
 {
 	int i, j;
 	for (j=0; j<20; j++)
 	{
 		for(i=0; i<10000; i++)
 		{
 			SET_BIT(PORTD, 0x08);	/*turn port d bit 3 on for 7 s*/
 		}
 	}
 	CLEAR_BIT(PORTD, 0x08);		/* turn port d bit 3 off*/
 	printf("Launch successfull");
 }
/****************************** launch ********************************/

void laun_seq(void)
/*******************************laun_seq************************************

 * Description
 * executes appropriate sequence of functions to launch rocket.
 * 
 * 
 *								
 *Usage:
 ************************************************************************/
{
	    start_pad();
      	search();
      	adjust_pad();
      	launch( );
}
/****************************** laun_seq *******************************/