/*--------------------------------------------------------------------*/
//
//	Final Project - Collision Avoiding Robot
//	Author:		Demian Raven and Man Chun Liu
//	Class:		CSE 444 Spring 2001
// 	File Name:	sonarobot.c
//	Date:		December 18th, 2001
//
/*--------------------------------------------------------------------*/

/*------------------------------------------------------------------------
//  LED DISPLAY OF MOTOR CONTROL
//  displayed in 8-segment led
//  case 1: both leds off - motor is off
// 	case 2: led 1 on, led 2 off - motor is forward
//	case 3: led 1 off, led 2 on - motor in reverse
//  case 4: both leds on - motor is off
-------------------------------------------------------------------------*/



#include 

/*------------------------ global constant ---------------------------*/

#define WAIT_FOR_NEXT_MEASUREMENT 34000	// about 100ms
#define MAX_WAIT 65535					// not use now
#define DELAY 130						// about 500us
#define TOO_CLOSE 450
#define MOTOR_CONSTANCY 3

/*------------------------ hardware macros ---------------------------*/

#define _ENABLE_INTERRUPTS { EA = 1; }
#define _DISABLE_INTERRUPTS { EA = 0; }

#define _ENABLE_TIMER2_INTERRUPT { ET2 = 1; }
#define _DISABLE_TIMER2_INTERRUPT { ET2 = 0; }

#define _TIMER2_ON { TR2 = 1; }
#define _TIMER2_OFF { TR2 = 0; }

#define _CLEAR_TIMER2_FLAGS { TF2 = 0; EXF2 = 0; }

#define INIT P1_5
#define HIGH 1
#define LOW 0
#define TRUE 1
#define FALSE 0

#define MOTOR1_INPUT_A RXD
#define MOTOR1_INPUT_B INT0
#define MOTOR2_INPUT_A T0
#define MOTOR2_INPUT_B RD

/*--------------------------- Global Data ----------------------------*/

static unsigned int sonarValue = 0;
static bit trigger = FALSE;
static bit ignore = FALSE;
static bit left_triggered = FALSE;
static bit right_triggered = FALSE;


/*----------------------- Function Prototypes ------------------------*/

void initialize();
void forward();
void spin_left();
void spin_right();

/*-------------------------- Main Function ---------------------------*/
void main(void) {
	// counter values
	int i=0;
	unsigned int j=0;

	// initializing timer 2
	initialize();

	// for debugging, ON button
	P2_3 = LOW; 	// for led
	trigger = FALSE;
	while (trigger == FALSE)
	{
		trigger = P2_1;// wait for button pressed
	}
	P2_3 = HIGH;
	
	// continuous loop
	while (TRUE)
	{
		
		// clear led display
		P3 |= 0xFF;

		//HERE: if left or right, then stop reversing.... go forward
		if (left_triggered == TRUE || right_triggered == TRUE) forward();

		// clear the triggers
		left_triggered = FALSE;
		right_triggered = FALSE;

		// number of times we loop before we clear the triggers - allows for 
		//	fine-tuning spinning away from obstacles
		for (i = 0; i < MOTOR_CONSTANCY; i++)
		{
			// INIT go high
			INIT = HIGH;
	
			// delay 500us
			ignore = TRUE;
			_TIMER2_ON;	
			for (j=0; j < DELAY; j++);
			ignore = FALSE;
	
			// sleep
			PCON |= 0x01;
	
			//HERE: if both then go to the right	(case 1,1)
			//HERE: if left then spin right		 	(case 1,0)
			//HERE: if right, then spin left		(case 0,1)
			//HERE: otherwise, go forward...  		(case 0,0)
			if (left_triggered == TRUE) spin_right();
			else
			{
				if (right_triggered == TRUE) spin_left();
				else forward();
		   	}
					
			// individual display of triggering of sonar's minimal distances
			P2_2 = (left_triggered == TRUE) ? LOW : HIGH;	
			P2_4 = (right_triggered == TRUE) ? LOW : HIGH;
		
			//dalay loop between sonar measurements
			for (j=0; j < WAIT_FOR_NEXT_MEASUREMENT; j++);
		}
	}
}

/*-------------------------- Sonar Top ISR ---------------------------*/
void sonar_top(void) interrupt 5 {
	
	// clear interrupt flags
	_CLEAR_TIMER2_FLAGS;

	// if correct time to read measuremets
	if (ignore == FALSE) {
		// NOTE!!! turn sonar off here
		sonarValue = ((RCAP2H << 8) + RCAP2L) >> 4;
		if (sonarValue < TOO_CLOSE)
		{
			// if detecting potential obstacle and P1_2 is low, 
			//   then it is the left sonar
			if (P1_2 == LOW)
			{	
				left_triggered = TRUE;
			}
			// otherwise P1_2 is high, it is the right sonar
			else right_triggered = TRUE;
		}
		_TIMER2_OFF;	// turn off timer2
		INIT = LOW;		// INIT go low
	
		// clear timer2 registers
		TH2 = 0x00;
		TL2 = 0x00;
		RCAP2H = 0x00;
		RCAP2L = 0x00;
	}

	return;
}


/*-------------------------- initialization --------------------------*/

void initialize() {

	PSW = 0x00;
	_DISABLE_INTERRUPTS;	// disable all interrupts
	T2CON |= 0x09;			// set timer 2 to negative-edge capture mode
	_ENABLE_INTERRUPTS;		// enable all interrupts
	_ENABLE_TIMER2_INTERRUPT;	// enable timer 2 interrupt

	INIT = LOW;				// set init to low
	//HERE: motor is stopped
	P3 = 0xFF;
}

void forward()
{
	MOTOR1_INPUT_A = LOW;
	MOTOR1_INPUT_B = HIGH;
	MOTOR2_INPUT_A = HIGH;
	MOTOR2_INPUT_B = LOW;
}

void spin_right()
{
	MOTOR1_INPUT_A = LOW;
	MOTOR1_INPUT_B = HIGH;
	MOTOR2_INPUT_A = LOW;
	MOTOR2_INPUT_B = HIGH;
}

void spin_left()
{
	MOTOR1_INPUT_A = HIGH;
	MOTOR1_INPUT_B = LOW;
	MOTOR2_INPUT_A = HIGH;
	MOTOR2_INPUT_B = LOW;
}

/*--------------------------------------------------------------------*/
// end sonarobot