/*************************************************
 *
 * Title:		gopher.c
 * Programmer:	dave rea 
 * Date:	19 ap 2001
 * Version:	1.0
 *
 * Description:
 *	Gopher tries to collect 3 pucks while avoiding falling
 *	off the table.  His two IR detectors facing down avoid
 *	edges.  His laser and CDS cells detect the presence of
 *	pucks.  Once a puck is picked up, he looks for the
 *	IR beacon to drop it.
 *
 *	The robot will wait for the bumper to be sensed to begin
  *************************************************/

/**********includes********************************/
#include <analog.h>
#include <motortjp.h>
#include <clocktjp.h>
#include <isrdecl.h>
#include <vectors.h>
#include <stdio.h>
#include <servotjp.h>
/**********end of includes***************************/

/**********constants*******************************/
#define LEFT_MOTOR	0
#define RIGHT_MOTOR	1
#define GRIP_MOTOR	0
#define LIFT_MOTOR  1
#define MAX_SPEED	100
#define RIGHT_RETARD 0
#define LEFT_RETARD  0
#define ZERO_SPEED	0
#define AVOID_THRESHOLD	100
#define OPEN	1500
#define CLOSE	3800
#define UP		2500
#define DOWN 	1500
#define BUMPER		analog(0)
#define CDS_CELLS	analog(4)
#define RIGHT_IR	analog(5)
#define LEFT_IR		analog(6)
#define RIGHT_EYE	analog(2)
#define CENTER_EYE	analog(1)
#define LEFT_EYE	analog(3)
#define TURN_180_TIME	900
#define TURN_120_TIME	600
#define TURN_90_TIME	450
#define	FORWARD	0
#define RIGHT	1
#define LEFT	2
#define NO	0
#define YES	1
#define FORWARD_TIME		100
#define SLIGHT_TURN_TIME	50
#define EDGE_TURN_TIME		200
#define CREEP	10
#define GRIP_TIME 600
#define CDS_2_3_THRESHOLD	117
#define CDS_1_2_THRESHOLD	91
#define CDS_0_1_THRESHOLD	66
#define CLOSE_TO_BEACON		120
/************end of constants***********************/

/************prototypes***************************/
void seek_puck();
void seek_beacon();
void turn_random(void);
void turn_from_edge();
//void turn_from_beacon();
void lock_on_puck();
void creep_forward();
void turn_90_right();
void turn_90_left();
void turn_120();
void turn_180();
void motors_stop();
void grip_puck();
void drop_puck();
void step_forward();
void step_right();
void step_left();
void initialize();
void check_eyes();
//void spin();


/*************end of prototypes********************/

/************global variables**************************/
int irdr, irdl, got_puck, heading, cds, x, score=0;
int right_eye, left_eye, center_eye;
int grossly_aligned;
int align_tries=0;

/*************end of global variables******************/


void main(void)
{
	
	init_analog();
	init_motortjp();
	init_clocktjp();
	init_servotjp();

	*(unsigned char *) 0x7000=0x07;	/* turn on the IR emmitters */

	while(BUMPER<120);		/* press the rear bumper to start */
	
	initialize();
	wait(1000);

	while(score < 3)
	{
		seek_puck();
		seek_beacon();
		drop_puck();
		got_puck = NO;
		score++;
		turn_180();
		
		motors_stop();
		wait(2000);
	}//end of while score<3
}
/**********end of main******************************/

void check_eyes()
/*************************************************
 *
 * Function
 *	reads the eye analog values
 *
 * Inputs:
 *	parameters:	none
 * 	globals:	the three eyes
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	none
 * Notes:
  *************************************************/
{
	right_eye 	= RIGHT_EYE;
	left_eye	= LEFT_EYE;
	center_eye	= CENTER_EYE;
}
/***************end of check_eyes**********************/


void creep_forward()
/*************************************************
 *
 * Function
 *	tries to move forward one "car-length",
 *	not expected to be accurate.
 *	creeps in case edge is there
 * Returns:	none
 * Inputs:
 *	parameters:	CREEP
 * 	globals:	irdr, irdl
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	step_forward(), turn_from_edge()
 * Notes:
  *************************************************/
{
	int j=0;
	
	irdr = RIGHT_IR;
	irdl = LEFT_IR;
   	while(((irdl < AVOID_THRESHOLD) && (irdr < AVOID_THRESHOLD)) || (j<CREEP))
	{
		step_forward();
		irdr= RIGHT_IR;
		irdl = LEFT_IR;
		j++;
	}
	if ((irdl < AVOID_THRESHOLD) || (irdr < AVOID_THRESHOLD))
			{
				turn_from_edge();
			}
	
}
/***************end of creep_forward**********************/

void drop_puck()
/*************************************************
 *
 * Function
 *	puts the puck down, and backs away
 * Returns:	none
 * Inputs:
 *	parameters:	GRIP_TIME, MAX_SPEED, OPEN  DOWN
 * 	globals:	none
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), motors_stop(), wait()
 * Notes:
  *************************************************/
{
	motors_stop();
	wait(2000);
	servo(LIFT_MOTOR, DOWN);
	wait(600);
	servo(GRIP_MOTOR, OPEN);
	wait(600);
	motorp(LEFT_MOTOR, -MAX_SPEED);
	motorp(RIGHT_MOTOR, -MAX_SPEED);
	wait(GRIP_TIME);
	motors_stop();
}
/***************end of drop_puck**********************/

void grip_puck()
/*************************************************
 *
 * Function
 *	goes forward a bit to entrap puck, then grips,
 *	lifts
 * Returns:	none
 * Inputs:
 *	parameters:	GRIP_TIME, MAX_SPEED, CLOSE, UP
 * 	globals:	got_puck
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), motors_stop(), wait()
 * Notes:
  *************************************************/
{
	motors_stop();
	wait(2000);
	motorp(LEFT_MOTOR, MAX_SPEED);
	motorp(RIGHT_MOTOR, MAX_SPEED);
	wait(GRIP_TIME);
	motors_stop();
	servo(GRIP_MOTOR, CLOSE);
	wait(600);
	servo(LIFT_MOTOR, UP);
	wait(600);
	
	got_puck = YES;
}
/***************end of grip_puck**********************/

void initialize()
/*************************************************
 *
 * Function
 *	intializes gopher
 * Returns:	none
 * Inputs:
 *	parameters:	OPEN, DOWN, NO
 * 	globals:	got_puck
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motors_stop()
 * Notes:
  *************************************************/
{
	motors_stop();
	got_puck=NO;
	servo(GRIP_MOTOR, OPEN);
	servo(LIFT_MOTOR, DOWN);

}
/***************end of motors_stop**********************/

void lock_on_puck()
/*************************************************
 *
 * Function
 *	orients gopher when puck detected.
 *	grips puck if in front of gopher.
 * Returns:	none
 * Inputs:
 *	parameters:	NO, FORWARD, FORWARD_TIME
 * 	globals:	heading, cds, x,
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motors_stop(), grip_puck(), step_forward()
 *			step_right(), step_left(), turn_90_right(),
 *			turn_90_left()
 * Notes:
  *************************************************/
{
	if (heading == FORWARD)
	{	step_forward();
		cds = CDS_CELLS;
		if ((cds < CDS_1_2_THRESHOLD) && (cds > CDS_0_1_THRESHOLD)){
			grip_puck();}
	}
	if (heading == RIGHT)
	{
		step_right();
		cds = CDS_CELLS;
		if ((cds < CDS_0_1_THRESHOLD))
		{	
			step_left();
			creep_forward();
			turn_90_right();
			creep_forward();
			turn_90_right();
			x=0;//reinitiallizes random_turn
		}
	}
	if (heading == LEFT)
	{
		step_left();
		cds = CDS_CELLS;
		if ((cds > CDS_2_3_THRESHOLD))
		{	
			step_right();
			creep_forward();
			turn_90_left();
			creep_forward();
			turn_90_left();
			x=0;//reinitiallizes random_turn
		}
	}
}
/***************end of lock_on_puck**********************/

void motors_stop()
/*************************************************
 *
 * Function
 *	stops both drive motors
 * Returns:	none
 * Inputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp()
 * Notes:
  *************************************************/
{
	motorp(LEFT_MOTOR, ZERO_SPEED);
	motorp(RIGHT_MOTOR, ZERO_SPEED);
}
/***************end of motors_stop**********************/

void seek_beacon()
/*************************************************
 *
 * Function
 *	locates direction of beacon and goes a bit toward it
 * Returns:	none
 * Inputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), check_eyes(),
 * 						step_right(), step_left(),
 * Notes:
  *************************************************/
{
	grossly_aligned = NO;
	check_eyes();
	while ((center_eye<CLOSE_TO_BEACON) && (right_eye<CLOSE_TO_BEACON) && (left_eye<CLOSE_TO_BEACON))
	{
	
		if (grossly_aligned == NO)
		{
			while((center_eye-5 <= right_eye) || (center_eye-5 <= left_eye))
			{
				step_right();
				check_eyes();
				wait(600);
			}
			grossly_aligned = YES;
		}
		while (((center_eye-left_eye<0) || (center_eye-right_eye<0)) && (align_tries<5))
		{
			if (center_eye-right_eye<0)
			{	step_right();
				wait(600);
			}
			else
			{
				step_left();
				wait(600);
			}
			check_eyes();
			align_tries++;
			if((center_eye>CLOSE_TO_BEACON) || (right_eye>CLOSE_TO_BEACON) || (left_eye>CLOSE_TO_BEACON))
				{
				align_tries=6;
				} 
		}
		align_tries=0;
		step_forward();
		check_eyes();
		if ((center_eye<right_eye) && (center_eye<left_eye))
		{
		center_eye=200;//drops puck if too close to beacon
		}	
	}	
	
}
/***************end of seek_beacon**********************/

void seek_puck()
/*************************************************
 *
 * Function
 *	searches the table for a puck until it gets one.
 *	occasional random turn when on table.
 * Returns:	none
 * Inputs:
 *	parameters:	FORWARD_TIME, MAX_SPEED, CDS_CELLS,
 *				RIGHT_IR, LEFT_IR
 * 	globals:	irdl, irdr, cds
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), motors_stop(), step_forward(),
			lock_on_puck(),
 * Notes:
  *************************************************/
{
	int i;
	unsigned random_time;
	while(got_puck == NO)
	{
		random_time=TCNT;
		i=((random_time%1024)%70);
		
		if (i<30)
		{
		i=30;
		}
		
		for(x=0; x<i; x++)
		{
		
		irdr = RIGHT_IR;
		irdl = LEFT_IR;
		if ((irdl < AVOID_THRESHOLD) || (irdr < AVOID_THRESHOLD)){
			motors_stop();
			turn_from_edge();}
		
		
		cds = CDS_CELLS;
		if (cds<CDS_2_3_THRESHOLD){
			lock_on_puck();}
		if (got_puck == YES){
				x=i;}

		if (got_puck == NO){
		step_forward();
		heading=FORWARD;}

		cds = CDS_CELLS;
		if (cds<CDS_2_3_THRESHOLD){
			lock_on_puck();}
		
		//IF NEAR BEACON, TURN FROM BEACON
		check_eyes();
		if((center_eye+1>CLOSE_TO_BEACON) || (right_eye+1>CLOSE_TO_BEACON) || (left_eye+1>CLOSE_TO_BEACON))
		{
			turn_120();
		} 
		
		if (got_puck == YES){
				x=i;}
		}//end of for loop
		motors_stop();	//here's where he appears to change
		wait(1000);		//direction on his own
		if (got_puck == NO)
		{
			turn_random();
		}
	
	}//end of while got_puck=NO
	
}
/***************end of seek_puck**********************/



//void spin()
/*************************************************
 *
 * Function
 *	spins in a circle
 * Returns:	none
 * Inputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp()
 * Notes:
  *************************************************/
//{
//	
//	motorp(LEFT_MOTOR, MAX_SPEED);
//	motorp(RIGHT_MOTOR, -MAX_SPEED);
//	
//}
/***************end of spin**********************/


void step_forward()
/*************************************************
 *
 * Function
 *	steps boldly forward for a short time, then stops
 * Returns:	none
 * Inputs:
 *	parameters:	FORWARD_TIME, MAX_SPEED
 * 	globals:	none
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), motors_stop()
 * Notes:
  *************************************************/
{
	motorp(LEFT_MOTOR, MAX_SPEED-LEFT_RETARD);
	motorp(RIGHT_MOTOR, MAX_SPEED-RIGHT_RETARD);
	wait(FORWARD_TIME);
	//motors_stop();
	//wait(FORWARD_TIME);
}
/***************end of step_forward**********************/

void step_left()
/*************************************************
 *
 * Function
 *	turns boldly left for a short time, then stops
 * Returns:	none
 * Inputs:
 *	parameters:	SLIGHT_TURN_TIME, MAX_SPEED
 * 	globals:	none
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), motors_stop(), wait()
 * Notes:
  *************************************************/
{
	motorp(LEFT_MOTOR, -MAX_SPEED);
	motorp(RIGHT_MOTOR, MAX_SPEED);
	wait(SLIGHT_TURN_TIME);
	motors_stop();
}
/***************end of step_left**********************/

void step_right()
/*************************************************
 *
 * Function
 *	turns boldly right for a short time, then stops
 * Returns:	none
 * Inputs:
 *	parameters:	SLIGHT_TURN_TIME, MAX_SPEED
 * 	globals:	none
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), motors_stop(), wait()
 * Notes:
  *************************************************/
{
	motorp(LEFT_MOTOR, MAX_SPEED);
	motorp(RIGHT_MOTOR, -MAX_SPEED);
	wait(SLIGHT_TURN_TIME);
	motors_stop();
}
/***************end of step_right**********************/

void turn_120()
/*************************************************
 *
 * Function
 *	tries to turn about 120 degrees to the left.
 *	not expected to be accurate.
 * Returns:	none
 * Inputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), wait(), motors_stop()
 * Notes:
  *************************************************/
{
	motorp(LEFT_MOTOR, -MAX_SPEED);
	motorp(RIGHT_MOTOR, MAX_SPEED);
	wait(TURN_120_TIME);
	motors_stop();
}
/***************end of turn_120**********************/

void turn_180()
/*************************************************
 *
 * Function
 *	tries to turn about 180 degrees to the left.
 *	not expected to be accurate.
 * Returns:	none
 * Inputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), wait(), motors_stop()
 * Notes:
  *************************************************/
{
	motorp(LEFT_MOTOR, -MAX_SPEED);
	motorp(RIGHT_MOTOR, MAX_SPEED);
	wait(TURN_180_TIME);
	motors_stop();
}
/***************end of turn_180**********************/


void turn_from_edge()
/*************************************************
 *
 * Function
 *	slight turn away from edge.
 *	not expected to be accurate.
 * Returns:	none
 * Inputs:
 *	parameters:	SLIGHT_TURN_TIME, ZERO_SPEED, MAX_SPEED
 * 	globals:	irdr, irdl
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), wait(), motors_stop(), turn_random()
 *						turn_120(),
 * Notes: expecting motors stopped on entry
  *************************************************/
{
	//motors_stop();
	
	irdr= RIGHT_IR;
	irdl = LEFT_IR;
		if ((irdl < AVOID_THRESHOLD)  && (irdr < AVOID_THRESHOLD))
			{
				//motors_stop();
				wait(2000);
				turn_120();
				wait(600);
								
			}
		
		if ((irdr < AVOID_THRESHOLD) && (irdl >= AVOID_THRESHOLD))
			{
				
				motorp(RIGHT_MOTOR, ZERO_SPEED);
				motorp(LEFT_MOTOR, -MAX_SPEED);
				wait(EDGE_TURN_TIME);
			}
		
		if ((irdl < AVOID_THRESHOLD) && (irdr >= AVOID_THRESHOLD))
			{
				
				motorp(LEFT_MOTOR, ZERO_SPEED);
				motorp(RIGHT_MOTOR, -MAX_SPEED);
				wait(EDGE_TURN_TIME);
			}
		
	
	motors_stop();
}
/***************end of turn_from_edge**********************/

void turn_90_left()
/*************************************************
 *
 * Function
 *	tries to turn about 90 degrees to the left.
 *	not expected to be accurate.
 * Returns:	none
 * Inputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), wait(), motors_stop()
 * Notes:
  *************************************************/
{
	motorp(LEFT_MOTOR, -MAX_SPEED);
	motorp(RIGHT_MOTOR, MAX_SPEED);
	wait(TURN_90_TIME);
	motors_stop();
}
/***************end of turn_left**********************/

void turn_random(void)
/*************************************************
 *
 * Function
 *	will turn in a random direction for a "random" amount of
 *	time, dictated by the fast change in the lower bits in
 *	milliseconds()./
 * Retunrs:	none
 * Inputs:
 *	parameters:	LEFT, RIGHT, MAX_SPEED
 * 	globals:	heading
 * 	registers:	TCNT
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), wait(), motors_stop()
 * Notes:
  *************************************************/
{
	int i;
	unsigned rand;
	rand = TCNT;

				motors_stop();
				wait(600);
				
	if (rand & 0x0001)
	/* turn left */
	{
			motorp(LEFT_MOTOR, -MAX_SPEED);
			motorp(RIGHT_MOTOR, MAX_SPEED);
			heading=LEFT;
	}
	else
	/*turn right*/
	{
			motorp(LEFT_MOTOR, MAX_SPEED);
			motorp(RIGHT_MOTOR, -MAX_SPEED);
			heading=RIGHT;
	}

	i=(rand % 1024);
	if (i>250) wait(i); else wait(250);
	
	motors_stop();
}
/***************end of turn_random**********************/

void turn_90_right()
/*************************************************
 *
 * Function
 *	tries to turn about 90 degrees to the right.
 *	not expected to be accurate.
 * Returns:	none
 * Inputs:
 *	parameters:	MAX_SPEED, TURN_90_TIME
 * 	globals:	none
 * 	registers:	none
 * Outputs:
 *	parameters:	none
 * 	globals:	none
 * 	registers:	none
 * Functions called: 	motorp(), wait(), motors_stop()
 * Notes:
  *************************************************/
{
	motorp(LEFT_MOTOR, MAX_SPEED);
	motorp(RIGHT_MOTOR, -MAX_SPEED);
	wait(TURN_90_TIME);
	motors_stop();
}
/***************end of turn_right**********************/