#include <avr/io.h>
#include <util/delay.h>
#include "common.h"
#include "servos.h"

void init_servos(void)
{
    SET(SERVO_DDR, SERVO_L_BIT);
    SET(SERVO_DDR, SERVO_R_BIT);

    SET(TCCR0A, COM0A1); // Clear OC0A on Compare Match, set OC0A at BOTTOM
    SET(TCCR0A, COM0B1); // Clear OC0B on Compare Match, set OC0B at BOTTOM

    // Fast PWM, 0xFF TOP
    SET(TCCR0A, WGM00);
    SET(TCCR0A, WGM01);

    // Prescaler. clkPWM = clkIO / 64
    SET(TCCR0B, CS00);
    SET(TCCR0B, CS01);
}

void run_servos(void)
{
    SERVO_L_REG = US2TIMER0(SERVO_MAX);
    SERVO_R_REG = US2TIMER0(SERVO_MID);
    _delay_ms(10000); // 10s
}