#include <avr/io.h>
#include <math.h>
#include <avr/interrupt.h>
#include "header.h"
#include <avr/pgmspace.h>
#include <ctype.h>
#include <avr/sfr_defs.h>


// Routine definition
void InitPorts(void);
void USART_Init(unsigned	int	ubrr);
void update_values(void);
void Timers_Init(void);
void external_interrupts_init(void);
void adc_init(void);
void software_servo_int(void);


// Global variables

static u8 *TX_buffer;
static u8 TX_data_to_send[TX_buffer_length];


volatile static u8 TX_read;
static u8 RX_buffer[RX_buffer_length];
static u8 *Received_data;

volatile static u8 RX_write;
static u8 	receive_full=0,fresh_data=0;				//Receive Flag

static u8	send_data=1;//send_data=0;
static u8  	number=0;
static u8 temp;
// Accelerometers
#include "files/adc_accelerometers.c"					//ADC kai Accelerometers Variables

#include "files/interrupt_routines.c"
#include "files/servos.h"								// Se poies 8eseis frenaroun ta servo
//#include "files/stability.c"
//--------------------------------main-----------------------------------
int main(void)
{
	sei();
	adc_init();
	InitPorts();
	external_interrupts_init();

	USART_Init(MYUBRR);	//115.2K
	Timers_Init();	
	static 	u8 servo_propotion=0;				// Poso na kinh8oun ta servo tou timoniou
	static 	u8 esp_on=0;

	while(1)
	{
		
		/*if (receive_full==1)
		{
			TX_buffer=&RX_buffer[0];
			//TX_buffer=&IDN[0];
			//TX_buffer=pgm_read_byte(&IDNT[0]);

			UDR=TX_buffer[0];			//Trigger transmition by loading UDR
			receive_full=0;
		}*/
		if ((send_data==1))//&&(fresh_data==1))
		{
			//update_values();
			TX_buffer=0;
			//TX_buffer=Received_data;
			UDR=TX_buffer[0];			//Trigger transmition by loading UDR
			
			send_data=0;
			fresh_data=0;

		}

		if(!(ADCSRA&0b01000000))			// An oloklhrw8hke to Conversion  xekina ena neo, To ADSC phge sto 0
			adc_routine();				// Jekina neo Conversion
			
		// 3_12_07
		
		if (watchdog_timer<100)
			watchdog_timer++;
		if (watchdog_timer>50){				// Mpei edw mesa, den exei labei poly wra dedomena. Mhdennizetai sthn receive
				front_left_servo=47;	// Kentrare ta servo
				front_right_servo=47;
				back_left_servo=47;
				back_right_servo=46;	// Sto 47 frenarei kai mas xalaei ton akolou8o elegxo
				esp_on=0;
				//throttle_servo=47;
				//steer_servo=47;
		}
		if((front_left_servo==front_left_servo_brake)||(front_right_servo==front_right_servo_brake)||(back_left_servo==back_left_servo_brake)||(back_right_servo==back_right_servo_brake))
			esp_on=1;		// An hmaste edw shmainei pws douleyei to ESP;
		else
			esp_on=0;
		if((throttle_servo<41)&&(throttle_servo>20)&&(esp_on==0)){			//  Boh8hse sto frenarisma
				servo_propotion=(43-throttle_servo);						// Sto 43 einai sto kentro
				front_left_servo=front_left_servo_release+servo_propotion;	
				front_right_servo=front_right_servo_release-servo_propotion;
				back_left_servo=back_left_servo_release-2*servo_propotion;
				back_right_servo=back_right_servo_release+2*servo_propotion;

				watchdog_timer=45;			//
		}
		if((steer_servo_ser>23)&&(steer_servo_ser<63)&&(steer_servo>36)&&(steer_servo<50)) // An einai ola OK dwse thn entolh apo seiriakh
			steer_servo_out=steer_servo_ser;								
		else
			steer_servo_out=steer_servo;									// Alliws ayto pou metrhses

		// 3_12_07

		//stability();
		//software_servo_int();
		/*
#define front_left_servo_brake 57
#define front_left_servo_release 47
#define front_right_servo_brake 37
#define front_right_servo_release 47
#define back_left_servo_brake 33
#define back_left_servo_release 47
#define back_right_servo_brake 47
#define back_right_servo_release 33
*/


			
	}

}
//--------------------------------main-----------------------------------




void InitPorts(void){	

	// Disable Jtag in order to use PORTC5:2

	MCUCSR|=(1<<JTD);						// ATmega 16 sel 59 kai sel 232 
	MCUCSR|=(1<<JTD);						// ATmega 16 sel 59 kai sel 232

	DDRB&=~((1<<PB7)|(1<<PB6)|(1<<PB5)|(1<<PB4));	// PB4:7 inputs (Hall Effect Sensors)
	DDRB&=~((1<<PB3)|(1<<PB2));						// PB2:3 inputs (SteerIn PB3, Trothle_in PB2)	
	DDRB|=0b00000011;								// PB1:0 outputs (SteerOut PB1, ThottleOut PB0)

	DDRA=0;		// PortA input

	DDRD|=(0<<PD0)|(1<<PD1);			// PD0 RXD in, PD1 TXDoutcv
	DDRD|=(0<<PD2);						// PD2==>External Interrupt 0, in. Duty Cycle Accelerometer
	DDRD|=(0<<PD3);						// PD3==>External Interrupt 1, in. XOR from Hall Effect

	DDRC&=~((1<<PC6)|(1<<PC7));			// PC6:in, PC7:in;
	DDRC|=(1<<PC5)|(1<<PC4)|(1<<PC3)|(1<<PC2);	//PC2:5 out
}

void Timers_Init(void)
{
	// Timer Counter_0 8bit
	TCCR0|=(0<<CS02)|(0<<CS01)|(1<<CS00);		//No prescaling Timer prescaler T_overflow=16us
	TIMSK|=(1<<TOIE0);							//Timer/Counter 0, overflow enable
	
	// Timer Counter_2 8bit
	TCCR2|=(0<<CS22)|(1<<CS21)|(0<<CS20);		//Timer prescaler=8 T_overflow=128 us
	TIMSK|=(1<<TOIE2);							//Timer/Counter 2, overflow enable

}

void	USART_Init(unsigned	int	ubrr)
{
	//Set Baud rate here
	UBRRH=(unsigned char)(ubrr>>8);
	UBRRL=(unsigned char)ubrr;
	// Enable receiver and trasmitter and receiver and transmitter Interrupts
	UCSRB=(1<<RXEN)|(1<<TXEN)|(1<<RXCIE)|(1<<TXCIE);
	//Set frame format; 8 data, 1 stop bit sel 164
	UCSRC=(1<<URSEL)|(0<<USBS)|(1<<UCSZ1)|(1<<UCSZ0);
	//PORTB=~ubrr;

	TX_read=1;
	RX_write=0;
}



void update_values(void)
{
	TX_data_to_send[0]='F';
	TX_data_to_send[1]='L';
	TX_buffer=&TX_data_to_send[0];
	UDR=TX_buffer[0];			//Trigger transmition by loading UDR
	
}

void external_interrupts_init(void)
{
	GICR|=(1<<INT0)	;							//Enable external Interrupt 0
	MCUCR|=(0<<ISC01)|(1<<ISC00);				//Any logical change on Interrupt 0 generates Interrupt sel 67

	GICR|=(1<<INT1)	;							//Enable external Interrupt 1
	MCUCR|=(0<<ISC11)|(1<<ISC10);				//Any logical change on Interrupt 0 generates Interrupt sel 67

}

void adc_init(void){
	ADMUX&=~((1<<REFS0)|(1<<REFS1));			// REFS0=0 kai REFS1=0	ARef,Internal Vref off(sel 215)
	//ADMUX|=((1<<REFS0)|(1<<REFS1));				// REFS0=1 kai REFS1=1	ARef,Internal Vref on(sel 215)
	ADMUX&=~(1<<ADLAR);							// Left Adjust result off
	
	ADMUX&=ADC0_and;
	ADMUX|=ADC0_or;								// Start from ADC0 port

	ADCSRA|=(1<<ADIE)|(1<<ADEN);				// Interrupt Enable, ADC Enable
	//ADCSRA|=(0<<ADPS2)|(1<<ADPS1)|(0<<ADPS0);	// ADC prescaler=4 sel 218

	ADCSRA|=(1<<ADPS2)|(1<<ADPS1)|(0<<ADPS0);	// ADC prescaler=64 sel 218

	ADCSRA|=(1<<ADSC);							// Start Conversion
}





