#include "servo.h"

void Servo::enable() volatile
{
    DDRB |= _BV(PB2);
    
    // configure impulse timer 1
    TCCR1A = 0;
    TCCR1B = _BV(WGM12);
    TCCR1C = 0;
    TIMSK1 = _BV(OCIE1A);
    
    // configure repeatition timer 2
    TCCR2A = _BV(WGM21);
    TCCR2B = _BV(CS22) | _BV(CS21); // prescaler 256
    TIMSK2 = _BV(OCIE2A);
    OCR2A = 124;
    repetition_counter = 0;
    
    OCR1A = 1500; // 1,5 ms init value
}
    
void Servo::disable() const volatile
{
    PORTB &= _BV(PB2);
    DDRB &= _BV(PB2);
    TCCR1B &= ~PRESCALER_IMPULSE; // stop timer 1
    TCCR2B = 0; // stop timer 2
}
    
void Servo::setPosition(uint16_t pos) const volatile
{
    OCR1A = pos;
}

void Servo::handleTimer1(void) const volatile
{
    PORTB &= ~_BV(PB2); // end pulse (set pin low)
    TCCR1B &= ~PRESCALER_IMPULSE; // stop timer 1
}

void Servo::handleTimer2(void) volatile
{
    repetition_counter++;
    if(repetition_counter >= 5)
    {
        repetition_counter = 0;
        TCCR1B |= PRESCALER_IMPULSE; // start timer 1
        PORTB |= _BV(PB2); // start impuls (set pin high)
    }
}