#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include "driver.h"





ISR (TIMER1_OVF_vect)
{
 ADMUX  = 0x45;  //select adc input 0
 ADCSRA |= 0x40;
 loop_until_bit_is_set(ADCSRA, 4);
 signed int servo_pos = ADCW-512;

 if(servo_ref >  300) servo_ref = 300;
 if(servo_ref < -300) servo_ref = -300;

 signed int servo_speed = (servo_pos - servo_ref) * 5;

 unsigned int i = abs(servo_speed);
 if(i > 511) i = 511;
 OCR1A = i;
 OCR1B = i;
 if(servo_speed > 0) DDRD = 0xAF;
 else				 DDRD = 0x5F;
}



void inline init_driver()
{
 //phase correct, jedna perioda PWM trva 512 cyklu processoru.
 TCCR1A = 0xa2; //top = 3FF
 TCCR1B = 0x01; //start Timer
 TIMSK1 = 0x01; //timer 1 interrupt sources

 PORTD = 0x00;
 DDRD  = 0xAF;
}