#include <Mega32.h>
#include <delay.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

//Back Direction Motor Range
#define forward 0x0c
#define halfforward 0x09
#define back 0x01
#define halfback 0x04
#define stop 0x0a

//Front Steering Motor range
#define right 0xc0
#define halfright 0x90
#define left 0x10
#define halfleft 0x40
#define straight 0xa0

unsigned char c_receive, c_high, c_low;
unsigned char steering_control, feedback;                                                 


interrupt [USART_RXC] void uart_isr(void) {
	c_receive = UDR;                             
}

//************************************************************************
void main(void)
{
	//set up the ports
	DDRA=0x00;	// PORT A is an input
	DDRB=0xff;	// PORT B is an output
	DDRC=0xff;	// PORT C is an output reserved for back motor
	DDRD=0xf8;  // PORT D 7->34 are ouputs and 2->0 are inputs
	
	//Initlaize Ports and pins
	PORTC = 0x7f;
	
	PORTD.7 = 1;
		
	//Initalize UART for RF revceiver
	UCSRB = 0x10; 	//RxD   (PORTD.0 
	UBRRL = 207;	//207 = Clk/(clk*BaudRate -1) = 16e6/(16*9600-1)
	UCSRB.7 = 1;
	
	//Initalize ADC for front motor feedback
	ADMUX = 0b11100000;	//AVcc for reference, ADLAR, PortA.0
  	ADCSR = 0b11000110; 	//AD enable, AD start, prescale 1/64
  
	
	//Intialize varibales and pins
	steering_control = 125;
	 
	//crank up the ISRs0+
	#asm
  		sei
	#endasm

	while (1) { 
	
		//Decode 8-bit singal into seperate Front and Back motor controls
		c_high = c_receive & 0xf0;
		c_low = c_receive & 0x0f;

		//Back Motor Control Mapping
		if (c_low==stop){ 
		     
			PORTC = 0x7f;
			PORTD.7 = 1;
		} else if (c_low==halfforward) {
			PORTC = 0xbf;
			PORTD.7 = 0;
		} else if (c_low==forward) {
			PORTC = 0xff;
			PORTD.7 = 0;
		} else if (c_low==halfback) {
			PORTC = 0x3f;
			PORTD.7 = 0;
		} else if (c_low==back) {
			PORTC = 0x00;
			PORTD.7 = 0;        
		}
		
		//Front Motor Control feedback mapping
		if (c_high==straight){ 
			steering_control = 130;
		} else if (c_high==halfleft) {
			steering_control = 105;
		} else if (c_high==left) { 
			steering_control = 85;
		} else if (c_high==halfright) {
			steering_control = 155;
		} else if (c_high==right) {
			steering_control = 175;      
		}
		
	 	feedback = ADCH; 		//Read feedback from ADC 
      		ADCSR = 0b11000110;	//Start a new ADC  
      	
    
          	//Determine how to turn front motor
      		if(feedback <= (steering_control - 6) && feedback <= 190) { //turn right 
    			PORTB = 0x00;
      	   		PORTD.6 = 1;
      	   		PORTD.5 = 0;
      	   		PORTD.4 = 1;
      	   		PORTD.3 = 0;
       		} else if(feedback >= (steering_control + 6) && feedback >= 50 ){ //turn left
	   		PORTB = 0x00;
	   		PORTD.6 = 0;
      	   		PORTD.5 = 1;
      	   		PORTD.4 = 0;
      	   		PORTD.3 = 1;
      		} else {						//no turn
      	   		PORTB = 0x01;													PORTD.6 = 0;
      	   		PORTD.5 = 0;
      	   		PORTD.4 = 0;
      	   		PORTD.3 = 0;
      		}
	}
}