#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <inttypes.h>
#include <stdbool.h>

#include "m16_hand.h"

char servo_init_flag = 0; 

void servo_init(void)
{
   DDRD |= 0x30;
   TCNT1H = 0x00;
   TCNT1L = 0x00;
   ICR1H  = 0x4E;
   ICR1L  = 0x20;     //top = icr1 = 625*32 
	
   OCR1A = 0;
   OCR1B = 0;
   TCCR1A = 0xA0 ;    // MODE 8 
   TCCR1B = 0x12 ;    // Prescale 8
   // TCCR1B |= 0x04 start time  
   TIMSK  &= 0xC3;    // no interrupt
} 

void m16_hand(int pulse_width)
{
   int offset;
   offset = pulse_width;
   if(servo_init_flag == 1)
    {
      OCR1A = offset;
    }
   else 
    {
      servo_init();    
	  OCR1A = offset;
	  servo_init_flag = 1;
    }  
}