52 lines
1.1 KiB
C++
52 lines
1.1 KiB
C++
#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)
|
|
}
|
|
}
|