/*
Henry Hsu
Oscar Ramirez
ECE 476 Final Project

PORT D	[Input from RC Reciever]
Pin		Hookup
0		Motor -
1		Motor +
2		Steering -
3		Steering +

PORT B	[Output from Board to Vehicle Motors]
Pin		Hookup
0		Forward
1		!Forward
2		Reverse
3		!Reverse
4		Left
5		!Left
6		Right
7		!Right

PORT C	[Output from Board to Sensor Motors]
Pin		Hookup
0		Left
1		!Left
2		Right
3		!Right

PORT A [Input/Output from sensor]
Pin Hookup
0		Echo
1   		Pulse Trigger
distance = 2/foot
distance increment ticks at about 1ms
get new distance about every 30ms
*/

#include <Mega163.h>
#include <stdio.h>
#include <stdlib.h>    

#define begin {
#define end }
#define t1 16    //left
#define t2 24   //right
#define t3 16
#define t4 66		// t4 is evade time about equals 2 s
#define t5 120		//Period of sensor readings
#define t6 5 		//distance check ticks
#define regular 1
#define checkLeft 2
#define checkRight 3
#define center 4
#define evade 5

//Direction output definitions
#define forward 0xa9
#define back 0xa6
#define left 0x9a
#define right 0x6a
#define forwardLeft 0x99
#define forwardRight 0x69
#define backLeft 0x96
#define backRight 0x66
#define still 0xaa

unsigned char distance, distanceLeft, distanceRight, controls;
unsigned char reload;
unsigned int time1, time2,time3;
unsigned int time4, time5,time6;
unsigned char status;

void safetyControl(void);
void detectDanger(void);
void initialize(void);
void pulse(void);
void detectecho(void);

interrupt [TIM0_OVF] void timer0_overflow(void)
begin   
	//reload to force 1 mSec overflow
	TCNT0=reload;
	
	//Decrement the timer if not equal to zero
	if (time5>0) 	--time5;
	if (time6>0)  	--time6;
end  

void main(void)
begin
	initialize();

	while(1)
	begin
		if (time5==0) pulse(); //pulsing through the sensor
		if (time6==0) detectecho();//detecting the echo, and therefore distance
	end
end

void safetyControl(void)
begin
	switch(status)
	begin
		case regular:	//the case where regular operation sends user commands to the motor
		begin
			controls = PIND;

			if (controls == 0)
				PORTB = still;
			else if (controls == 0x01)
				PORTB = forward;
			else if (controls == 0x02)
				PORTB = back;
			else if (controls == 0x04)
				PORTB = left;
			else if (controls == 0x08)
				PORTB = right;
			else if (controls == 0x05)
				PORTB = forwardLeft;
			else if (controls == 0x09)
				PORTB = forwardRight;
			else if (controls == 0x0a)
				PORTB = backRight;
			else if (controls == 0x06)
				PORTB = backLeft;
			else PORTB = still;

	   		time1 = t1;
			break;   
	   	end

		//the case where user input has been cut and we are now "seeking" a safe path
		case checkLeft:
		begin  
			if(time1>0)
	   		begin
				PORTC.0=1;	//turn on motor to rotate sensor left
				PORTC.1=0;
				PORTC.2=0;
				PORTC.3=1;
				PORTB=still;	
     			time1--;
	   		end
	   		else if(time1==0) 
			begin
		  		status=checkRight;
		  		time2=t2;
		  		PORTC.0=0;
		  		PORTC.1=1;
  		  		PORTC.2=1;
  		  		PORTC.3=0;
		  		distanceLeft = distance;
	   		end
 			break;
		end
		//testing the forward right direction
		case checkRight:
		begin
			if (time2>0)
			begin
				time2--;
				PORTC.0=0;	//turn on motor to rotate sensor right
				PORTC.1=1;
		   		PORTC.2=1;
		   		PORTC.3=0;
			end
			else
	   		begin
	   			PORTC.0=0;
	   			PORTC.1=1;
  		  		PORTC.2=0;
  		  		PORTC.3=1;
				distanceRight = distance;
				status=center;
				time3=t3;
			end
			break;
		end
		//state to re-center the sensor
		case center:
		begin
			if (time3>0)
			begin
				PORTC.0=1;
				PORTC.1=0;
				PORTC.2=0;
				PORTC.3=1;
				time3--;
			end
			else
			begin
				PORTC.0=0;
				PORTC.1=1;
				PORTC.2=0;
				PORTC.3=1;
				status=evade;
				time4=t4;
			end
			break;
		end
		case evade:
		begin
			if(time4>0) //take evasive action front left, front right, or backwards
			begin
				time4--;
				if (distanceLeft > distanceRight)
					PORTB=forwardLeft;
				else if (distanceLeft < distanceRight)
					PORTB=forwardRight;
				else 	PORTB=back;
					
			end
			else if (time4==0) //return to regular state
			begin
				PORTB=still;
				status=regular;
			end
			break;	
		end
	end
end

void detectDanger(void)
begin
	if ((distance != 0) && (distance <= 6) && (status==regular)) 
		status=checkLeft;	//dangerous object detected, go to next state
end

void detectecho(void)
begin
	time6=t6;
	if(PINA.0==1)
	begin
		distance=distance+1;	//incrementing the distance due to time delay
	end
end

void pulse(void)
begin
	time5=t5;		//setting the length of the pulse
	detectDanger();
	safetyControl();	//determine output to motors
	PORTA.1=1;		//pulsing
	distance=0;
	PORTA.1=0;
end

void initialize(void)
begin
	DDRD=0x00;		// PORT D is an input
	DDRB=0xff;		// PORT B is an ouput
	DDRC=0xff;		// PORT C is an ouput
	DDRA=0xfe;		// PORT A.0 is input else output
	PORTB=still;
	         
	distance=0;
	time1=0;
	time2=0;
	time3=0;
	time4=0;
	time5=t5;
	time6=t6;
	status=regular;
	reload=256-16;		//value for 1/4 Msec  
	TCNT0=reload;		//preload timer 1 so that is interrupts after 1/4 mSec.
	TCCR0=2;		//prescalar to 8
	TIMSK=1;

	//crank up the ISRs
	#asm
		sei
	#endasm
end