#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>

//timeout values for each task
#define begin {
#define end }
 
// task subroutines
void task1(void);  					//flip pulse at C0
void task2(void);					//calculate  the position of the servo
void initialize(void); 				//initialize all components 
          
volatile unsigned int time1; 		//timeout counter for task1
volatile unsigned int time2;		//timeout counter for task2
volatile unsigned int t1=150;		//pulse width (default 1.50 ms)
volatile unsigned int nextT1=150;	//pulse width for next iteration
volatile unsigned int t2=700;		//time period for each task 2 (7ms)
unsigned char servoOut;				//Ouput value to servo

volatile char Ain ; 				//raw A to D number
volatile char AinHist[32] = {128,128,128,128,128,128,128,128,
							128,128,128,128,128,128,128,128,
							128,128,128,128,128,128,128,128,
							128,128,128,128,128,128,128,128};	//Ain history for low pass filtering
volatile char AinHistIndex=1;									//index to current element in AinHist
volatile unsigned int  AinTotal=128*32;							//total of Ain history
volatile char AinLP=128;										//Low passed Ain
volatile char AinLast=128;										//Last Low passed Ain

//PID
volatile int center=0;				//center acceleration
volatile int error =0;				//deviation from center
volatile int integral=0;			//integral of errors
volatile int derivative=0;			//derivative of errors
volatile int temp=0;				//tempory variable buffer
volatile int PID=0;					//PID control value
         
//**********************************************************
//timer 0 compare ISR
ISR (TIMER0_COMPA_vect)
begin      
  //Increment the  time counters for tasks
  ++time1;
  ++time2;
end  

void init_adc(void) {	// function to initialize A to D converions
     
   //init the A to D converter 
   //channel zero/ left adj /Internal 5v reference
   ADMUX = (0<<REFS1)|(1<<REFS0)|(1<<ADLAR);
   //enable ADC and set prescaler to 1/128*16MHz
   //and clear interupt enable
   //and start a conversion
   ADCSRA = (1<<ADEN) | (1<<ADSC)+ 7;

     	
	//Dummy  read (throw erronous first read)
	Ain = ADCH;
	Ain= Ain;
	Ain;
	ADCSRA |= (1<<ADSC) ;
	
	//real conversion
	while((ADCSRA<<ADSC)&1);		//stall til read is ready
	Ain = ADCH;
	Ain= Ain;
	center=Ain;						//calibrate center acceleration
  	
}

//********************************************************** 
//Set it all up
void initialize(void)
begin
	//initialize adc
	init_adc();

	//set up the servo
  	DDRC = (1<<PORTC0) ;			// PORT C.0 is an ouput
           
  	//set up timer 0 for 10 uSec timebase 
  	TIMSK0= (1<<OCIE0A);			//turn on timer 0 cmp match ISR 
  	TCCR0B= 2; 						//set prescalar to divide by 8
	OCR0A = 20;  					//20 * 8 / 16Mhz = 10 us
  	TCCR0A= (1<<WGM01) ;			// turn on clear-on-match
    
  	//init the servo status 
  	servoOut=0x00;
 
  	//init the task timer
  	time1=t1; 
	time2=t2; 
  
  	//crank up the ISRs
  	sei();
end  


  
//**********************************************************          
//Task 1
void task1(void) 
begin  
	//toggle pulse to servo
  	servoOut = servoOut ^ 1 ;
  	PORTC = servoOut<<PORTC0 ;
end  
 

//**********************************************************          
//Task 2
void task2(void) 
begin
	
	//get the sample  
	Ain = ADCH;
	Ain= Ain;
	
	//start new conversion
	ADCSRA |= (1<<ADSC) ;
	
	//Digital LOW PASS FILTER Ain
	if(++AinHistIndex>31){AinHistIndex=0;}			//cycle through each element
	AinTotal+=-AinHist[AinHistIndex]+Ain;			//subtract FIFO value and add new Ain from total
	AinHist[AinHistIndex] = Ain;					//replace last Ain with new Ain
	AinLP=AinTotal>>5;								//compute Low pass filtered Ain
		
	//PID
	int newerror=AinLP-center;
	derivative=	newerror-error;
	error=newerror;
	integral+=error;						

	if(integral>2000) integral=2000;				//limit integral 
	else if(integral<-2000) integral=-2000;
	
	PID = .97*error+.0375*integral+10*derivative;
	
	//clip PID
	if(PID>100) PID=100;
	else if(PID<-100) PID=-100;
	
	t1 =(t1+ 150+PID)/2;							//compute new pulse width as avg of old t1 and (center position + PID)
													//avg smoothes out transisiton
end  
 
//**********************************************************       
//Entry point and task scheduler loop
int main(void)
begin  
  	initialize();
  
  	//main task scheduler loop 
  	while(1)
  	begin     
		if (time1>=t1){time1=0;	task1();}
		if (time2>=t2){time2=0;	task2();}
  	end
end