/***************************************************************************
 *
 * Servo
 *
 *
 */


#include "servo.h"

void servo_init(void) {
  init_servo_ports();
  set_servo_pos(0);
   
  ICR1 = 20000;  // 20 ms Period
  
  TCCR1A = 0x80; //OC1b disconnected;
				  //Set OC1a on compare match when upcounting.Clear OC1a on compare match when downcounting.
				  //34 = 00 for compatibility
				  // Mode 8 PWM, Phase and Frequency Correct change :ICR1
  TCCR1B = 0x12; // Mode 8 Preescaler 8;
} 

void set_servo_pos(int pos){
  if(pos<=-1000) pos = -1000;
  if (pos>=950) pos = 950;
  OCR1A = pos+1420;
}

int get_servo_pos(void){
  return OCR1A-1420;
}

void init_servo_ports(void){
  DDRD = (DDRD | 0x20);

}