//#include <avr/signal.h>
#include <string.h>
#include <avr/io.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#define F_CPU 1600000UL

#define USART_BUFFER_SIZE 10
/* Baudrate settings. Refer to datasheet for baud rate error.
   Note also maximun baud rate.
   br = baudrate, fosc = clock frequency in megahertzs */
#define USART_BAUDRATE(br, fosc) (fosc*125000/br-1)
#define		pi		  3.14159
#define R2D 180/pi //Set the constants for the output pins
#define BRAKE1 0
#define BRAKE1 0
#define DIR1 1
#define BRAKE2 3
#define DIR2 2
#define led 6


#include "rs232.h"
#include "init.h"
#include "pid.h"


int main(void){ ///Main block


	init();//Run the initialization block
	int top;
	int bottom;
	int diff;
	double output;
	int top_offset=511;//Pre-determined offsets
	int bottom_offset=508;//Pre-determined offsets
	
	while(1){
	

		
		
		
		ADMUX=0;//Switch to ADC channel 0
		ADCSRA |= (1<<ADSC);//start the conversion process 
		while(ADCSRA&_BV(ADSC));//wait for the conversion process
		top=ADC;//Record the top sensor
		ADMUX=1;//Switch to ADC channel 1
		ADCSRA |= (1<<ADSC);//start the conversion process
		while(ADCSRA&_BV(ADSC));//wait for the conversion process
		bottom=ADC;//Record the bottom sensor
		diff=(top-top_offset)-(bottom-bottom_offset);//this is the difference between the top and bottom sensors
		PID(diff);


	}
}