#define F_CPU 1000000UL
#include <avr/io.h>
#include <stdio.h>
#include <util/delay.h>
#include "lcd.h"

void init(void)
{
  lcd_init(LCD_DISP_ON);
  lcd_clrscr();
  lcd_gotoxy(0,0);
  
  lcd_puts("Servo control");
  
  DDRB  |= 0b00000010;
  
  ADMUX  = (0 << REFS1) | (1 << REFS0)  // AVcc as reference voltage
         | (0 << MUX3) | (0 << MUX2) | (1 << MUX1) | (1 << MUX0);
         
  ADCSRA = (1 << ADEN) | (1 << ADSC)    // enabling and starting the ADC
         | (0 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // 1 MHz/8 = 125 kHz clock
  
  ICR1   = 19999;   // TOP value 19999 or 20 ms between pulses
  OCR1A  = 1500;    // set to 0.5 ms pulses to start with
  TCCR1A = (1 << COM1A1) | (0 << COM1A0) // non inverting
         | (0 << COM1B1) | (0 << COM1B0) // ch B disconnected       
         | (1 << WGM11)  | (0 << WGM10); // select mode 14 = 1110
  TCCR1B = (1 << WGM13)  | (1 << WGM12)
         | (0 << CS12) | (0 << CS11) | (1 << CS10); // 1:1 prescaler
}


int main(void)
{
  char buffer[40];

  init();
  
  while (1)
  {
    ADCSRA |= ( 1 << ADSC);          // start conversion
    while (ADCSRA & (1 << ADSC)) {}  // wait until finished
    OCR1A = 500 + 2*ADC;             // scale ADC and set OCR1A
    
    sprintf(buffer, "ADC%4u OCR%4u", ADC, OCR1A);
    lcd_gotoxy(0,1); 
    _delay_ms(50);
    lcd_puts(buffer);              
  }
}