#include <pic.h>

__CONFIG(HS & WDTDIS & PWRTDIS & BORDIS & LVPDIS & WRTEN & DEBUGDIS & DUNPROT & UNPROTECT);
#define 	_XTAL_FREQ 10000000
#define		PORTBIT(adr, bit)	((unsigned)(&adr)*8+(bit))

#define 	LFT_PWM		CCPR1L	//PWM CCP1 RC2
#define 	RGT_PWM		CCPR2L	//PWM CCP2 RC1
#define 	MD_STP		0		//Motor Direction Stop
#define 	MD_FWD		1		//Motor Direction Forward
#define 	MD_REV		2		//Motor Direction Reverse
#define 	LEFT		0		//Motor Direction Forward
#define		RIGHT		1		//Speed Calibration Turn Speed Right

static bit MOTOR1 	@ PORTBIT(PORTC, 4);
static bit MOTOR2 	@ PORTBIT(PORTC, 5);
static bit MOTOR3 	@ PORTBIT(PORTC, 0);
static bit MOTOR4 	@ PORTBIT(PORTC, 3);
static bit LED1 	@ PORTBIT(PORTB, 0);
static bit LED2 	@ PORTBIT(PORTB, 1);
static bit LED3 	@ PORTBIT(PORTB, 2);
static bit LED4 	@ PORTBIT(PORTB, 3);

void pause(unsigned int i) {
	unsigned int j;

	for(j=0; j<i; j++) {
		__delay_ms(50);
	}
}

void init(void){
	unsigned int i;

	TRISA 	= 0b00001111;	//PortA all outputs but 0 input
	TRISB 	= 0b00010000;	//PortB pin 4 input for GO_BTN, all other output for LEDS
	TRISC 	= 0;			//PortC all outputs
	PORTB 	= 0;			//PortB all low
	PORTC 	= 0;			//PortC all low
	INTCON=0;

	CCP1CON = 0b00001111; 	//Enable PWM mode on RC2
	CCP2CON = 0b00001111; 	//Enable PWM mode on RC1
	//PR2 	= 126;				//Top value for PWM
	//T2CON 	= 0b00000011; 	//Prescaler=16
	//Get rid of that blasted humming??
	PR2		= 255;			//Top value for PWM
	T2CON 	= 0b00000000; 	//Prescaler=1
	
	TMR2ON 	= 1;			//Enable TIMER2 for PWM
	ADCON1 	= 0b10000110;	//Set up ADC ports

	unsigned int j;
	for(j=0; j<10; j++) {
		PORTB=0b00000000;
		pause(5);
		PORTB=0b00001111;
		pause(5);
	}
}

unsigned int getPing(void) {
	//Get range value from Parallax PING))) Ultrasonic sensor

	//set pin high for 5 uS
	TRISB7 = 0;
	RB7 = 1;
	__delay_us(5);
	RB7 = 0;

	//wait holdoff period of 750uS
	TRISB7 = 1;
	__delay_us(750);
	
	while (!RB7) {}
	
	unsigned int i=0;
	while (RB7) {
		i++;
	}	

	TRISB7 = 0;
	RB7 = 0;

	return (i);
}

unsigned int getChannel(unsigned int ch) {
	//Get value from HAS-O3MB RC Reciever

	int i=0;
	
	//may be slightly higher, so cut off under/above
	//returned ranges for each channel
	//CH1 250-443 (left/right)
	//CH2 240-443 (up/down)
	//CH3 230-435 (throttle)

	if (ch==1) {
		while (RA1) {} 		//wait til lo
		while (!RA1) {} 	//wait til hi
		while (RA1) {
			i++;
		}
		if (i<250) i=250;
		if (i>443) i=443;
		i=i-250;
		i=i*1.315;
	} 
	if (ch==2) {
		while (RA2) {} 		//wait til lo
		while (!RA2) {} 	//wait til hi
		while (RA2) {
			i++;
		}
		if (i<240) i=240;
		if (i>443) i=443;
		i=i-240;
		i=i*1.251;
	}
	if (ch==3) {
		while (RA3) {} 		//wait til lo
		while (!RA3) {} 	//wait til hi
		while (RA3) {
			i++;
		}
		if (i<230) i=230;
		if (i>435) i=435;
		i=i-230;
		i=i*1.243;
	}

	return i;
}

void motor_ch_dir(unsigned int side, unsigned int dir, unsigned int spd) {
	if (side==LEFT) {
		LFT_PWM = spd;
		if (dir==MD_STP) {
			MOTOR1 	= 0;	//Turn left motor FWD
			MOTOR2 	= 0;
		}
		if (dir==MD_FWD) {
			MOTOR1 	= 0;	//Turn left motor FWD
			MOTOR2 	= 1;
		}
		if (dir==MD_REV) {
			MOTOR1 	= 1;	//Turn left motor FWD
			MOTOR2 	= 0;
		}
	}
	if (side==RIGHT) {
		RGT_PWM = spd;
		if (dir==MD_STP) {
			MOTOR3 = 0;	//Turn right motor FWD
			MOTOR4 = 0;
		}
		if (dir==MD_FWD) {
			MOTOR3 = 0;	//Turn right motor FWD
			MOTOR4 = 1;
		}
		if (dir==MD_REV) {
			MOTOR3 = 1;	//Turn right motor FWD
			MOTOR4 = 0;
		}
	}	
}	

void main(void){
	init();
	unsigned int c1;
	unsigned int c2;

	for(;;) {
		c1 = getChannel(1);
		if (c1>128)
			{ motor_ch_dir(LEFT,MD_FWD,(c1-128)*2); LED1=1; LED2=0; }
		else
			{ motor_ch_dir(LEFT,MD_REV,(128-c1)*2); LED1=0; LED2=1; }
	
		c2 = getChannel(2);
		if (c2>128)
			{ motor_ch_dir(RIGHT,MD_FWD,(c2-128)*2); LED3=1; LED4=0; }
		else
			{ motor_ch_dir(RIGHT,MD_REV,(128-c2)*2); LED3=0; LED4=1; }
	}
}