/*
Author: Chris @ http://www.pyroelectro.com
Date: 2/3/2008

Program:
This program makes a servo pan back and forth and takes
data at 5 different points, interprets that data & displays
it on the output leds using PORTD & PORTC to talk with
the 74LS373's.
*/

//Initial Includes
#include <p18f452.h>
#include <timers.h>
#include <delays.h>
#include <adc.h>
#include <stdlib.h>

//Servo Initialization
int servo0 = 0xFC17;

//More Initializations
int radar_direction = 0;
int whichway = 0;
int count = 0;
int result = 0;
int round = 0;

//Function Declarations
void InterruptHandlerHigh (void);
int Ir_Data_Eval(int, int);

void main(void)
{

//Allow Interrupts
RCON = 0b000000000; 
INTCON = 0b10100000;

//Setup & Open A/D, Timers
OpenTimer0( TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_2 );
OpenTimer1( TIMER_INT_ON & T1_16BIT_RW & T1_SOURCE_INT & T1_PS_1_2 & T1_OSC1EN_OFF & T1_SYNC_EXT_OFF );

//Initialize Timers
WriteTimer0( 0xB1DF );
WriteTimer1( 0xFC17 );

//Initialize Port Directions
TRISD = 0x00;
TRISB = 0x00;
TRISC = 0x00;

//Initially Light Up All Leds & Turn Them Off
PORTB = 0xFF;
PORTD = 0xFF;
Delay10TCYx(100);
PORTD = 0x00;
Delay10KTCYx(100);

PORTB = 0x00;
PORTD = 0xFF;
Delay10TCYx(100);
PORTD = 0x00;
Delay10KTCYx(100);

//Infinite While Loop
	while(1)
	{
	
		//If radar is at one of 5 spots take a/d & output it to the leds
		switch(radar_direction)
		{
		
			case 0:
					
					ConvertADC();
					while( BusyADC() );
						result = ReadADC();
						
					PORTB = Ir_Data_Eval(result, 1);
					PORTD = 0x01;					
					Delay10TCYx(10);
					result = 0;
					PORTD = 0x00;
					break;
			case 1:

					ConvertADC();
					while( BusyADC() );
						result = ReadADC();
						
					PORTB = Ir_Data_Eval(result, 0);
					PORTD = 0x02;
					Delay10TCYx(10);
					result = 0;
					PORTD = 0x00;
					break;
			case 2:

					ConvertADC();
					while( BusyADC() );
						result = ReadADC();

					PORTB = Ir_Data_Eval(result, 0);
					PORTD = 0x04;
					Delay10TCYx(10);
					result = 0;
					PORTD = 0x00;
					break;
			case 3:

					ConvertADC();
					while( BusyADC() );
						result = ReadADC();
						
					PORTB = Ir_Data_Eval(result, 0);
					PORTD = 0x08;
					Delay10TCYx(10);
					result = 0;
					PORTD = 0x00;
					break;
			case 4:

					ConvertADC();
					while( BusyADC() );
						result = ReadADC();

					PORTB = Ir_Data_Eval(result, 0);
					PORTD = 0x10;
					Delay10TCYx(10);
					result = 0;
					PORTD = 0x00;
					break;
		}
	}
}

//INTERRUPT CONTROL
#pragma code InterruptVectorHigh = 0x08                //interrupt pointer address (0x18 low priority)
void InterruptVectorHigh (void)
{
        _asm        //assembly code starts
        goto InterruptHandlerHigh                //interrupt control
        _endasm //assembly code ends
}
#pragma code
#pragma interrupt InterruptHandlerHigh        //enf.

void InterruptHandlerHigh()        // declaration of InterruptHandler
{//this gets ran when ever the timers flop over from FFFF->0000
        if(INTCONbits.TMR0IF)                        //check if TMR0 interrupt flag is set
        {
				WriteTimer0( 0xB1DF );
				WriteTimer1( 0xFC17 );
				count = 0;
			    INTCONbits.TMR0IF = 0;                //clear TMR0 flag               
        }
        if(PIR1bits.TMR1IF == 1 && PIE1bits.TMR1IE == 1)        //if set controls the first servo
        {
		count++;	
				switch(count){		
			case 1:     PORTC = 0x01; // First Stage
					    WriteTimer1( servo0 );
						break;
			default:	PORTC = 0x00; // Left Gripper
						WriteTimer1(0);
						break;
				}

                PIR1bits.TMR1IF = 0;                        //clear Timer1 flag
                PIE1bits.TMR1IE = 1;                        //clear Timer1 enable flag set to zero

/*

Begin Direction Control

*/
	//Move the Servo In Small Steps
	if(whichway)
		servo0+=5;
	if(!whichway)
		servo0-=5;

	//When 90 degrees or 0 degrees is reached, switch direction
	if(servo0 <= 0xF82F ){
		whichway = 1;
		round++;
	}
	if(servo0 >= 0xFC17){
		whichway = 0;
		round++;
	}

/*

End Direction Control

*/

/*

Begin Radar Directional Lighting Control

*/

//If 120 < servo0 < 130
if(servo0 > 0xF8A7 && servo0 < 0xF8B1)
{
	if(whichway == 0)
		radar_direction = 0;
	if(whichway == 1)
		radar_direction = 1;
}

//If 370 < servo0 < 380
if(servo0 > 0xF9A1 && servo0 < 0xF9AB)
{
	if(whichway == 0)
		radar_direction = 1;
	if(whichway == 1)
		radar_direction = 2;
}

//If 620 < servo0 < 630
if(servo0 > 0xFA9B && servo0 < 0xFAA5)
{
	if(whichway == 0)
		radar_direction = 2;
	if(whichway == 1)
		radar_direction = 3;
}

//If 870 < servo0 < 880
if(servo0 > 0xFB95 && servo0 < 0xFB9F)
{
	if(whichway == 0)
		radar_direction = 3;
	if(whichway == 1)
		radar_direction = 4;
}
	
/*

End Radar Directional Lighting Control

*/

        }
    INTCONbits.GIE = 1;                                //re-enable all interrupts
}

int Ir_Data_Eval(int ir_data, int isCHIP0)
{
	//	@ 70cm+
if(ir_data > 0x171 && ir_data <= 0x19E){
	if(isCHIP0)
		return 0xC0;
	else
		return 0x40;
}
	//	@ 60cm+
else if(ir_data > 0x19E && ir_data <= 0x1C3 ){
	if(isCHIP0)
		return 0xA0;
	else
		return 0x20;
}
	//	@ 50cm+
else if(ir_data > 0x1C3 && ir_data <= 0x1EB ){
	if(isCHIP0)
		return 0x90;
	else
		return 0x10;
}

	//	@ 40cm+
else if(ir_data > 0x1EB && ir_data <= 0x20D ){
	if(isCHIP0)
		return 0x88;
	else
		return 0x08;
}

	//	@ 30cm+
else if(ir_data > 0x20D && ir_data <= 0x262 ){
	if(isCHIP0)
		return 0x84;
	else
		return 0x04;
}

	//	@ 20cm+
else if(ir_data > 0x262 && ir_data <= 0x2C6 ){
	if(isCHIP0)
		return 0x82;
	else
		return 0x02;
}

	//	@ 10cm+
else if(ir_data > 0x2DA){
	if(isCHIP0)
		return 0x81;
	else
		return 0x01;
}
	//If chip 0 make sure to turn on the very first led
if(isCHIP0)
	return 0x80;

return 0x00;
}