#include "servo.h"

namespace servo
{
    void init() {
        //Configure TIMER1
        TCCR1A|=(1<<COM1A1)|(1<<COM1B1)|(1<<WGM11); //NON Inverted PWM
        TCCR1B|=(1<<WGM13)|(1<<WGM12)|(1<<CS11)|(0<<CS10); //PRESCALER=1 MODE 14(FAST PWM)
        ICR1=19999; //fPWM=50Hz (Period = 20ms Standard).
        DDRD|=(1<<PD4)|(1<<PD5); //PWM Pins as Out
    }

    void pos(int pos) {
        OCR1A = pos;
    }
} // namespace servo