#define F_CPU 16000000L

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include "servocontrol.h"

#define TOP 500 // 1000*(64/16000000)

void servo_delay() {
   _delay_ms(1500);

}


void setup_servos() {
   DDRD = (1 << DDD5) | (1 << DDD6);
   TCCR0A |= (1 << COM0A1) | (1 << COM0A0) | (1 << WGM00) | (1 << COM0B1);
   TCCR0B |= (1 << CS00) | (1 << CS01);
   OCR0A = 0x00;

}

void move_servos() {
   for (int i = 100; i < TOP; i++) {
      OCR0A = i;
      servo_delay();
      if (OCR0A == 300) {
          PORTD &= ~ (1 << PD5);
        }

    }
}