#include "timer.h"
#include <math.h>
volatile unsigned long ms_count = 0;
void timer0_init()
{
sei();
TCCR0A = _BV(COM0A1) | _BV(COM0B1) |_BV(WGM01) | _BV(WGM00);
TCCR0B = _BV(CS01) | _BV(CS00);
TIMSK0 = _BV(TOIE0);
}
void timer1_init()
{
// set ovf at 50 hz for servos
ICR1H = 0x9C;
ICR1L = 0x3F;
//enable pwm and prescaler of 8
TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(COM1C1) | _BV(WGM11);
TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11);
// set data direction registers
DDRB = _BV(DDB5) | _BV(DDB6) | _BV(DDB7);
}
void servo1A(int angle)
{
int position = round(angle * 23.3 + 550);
OCR1AH = position >>8;
OCR1AL = position & 0x00FF;
}
void servo1B(int angle)
{
int position = round(angle * 23.3 + 550);
OCR1BH = position >>8;
OCR1BL = position & 0x00FF;
}
void servo1C(int angle)
{
int position = round(angle * 23.3 + 550);
OCR1CH = position >>8;
OCR1CL = position & 0x00FF;
}
void delay(unsigned long time)
{
ms_count = 0;
while (ms_count < time )
{
}
}
ISR(BADISR_vect)
{}
ISR(TIMER0_OVF_vect)
{
ms_count++;
}