Colony » Colony Scout » Colony Scout EE

#include <avr/io.h>

#include "servo.h"

#include <stdlib.h>

/*

servo controller. Timer0 is set to phase-correct PWM mode, with 32 prescaler.

SERVO_PWM1: PD6 (Timer0)

SERVO_PWM2: PD5 (Timer0)

*/

int8_t servo1_angle;

int8_t servo2_angle;

void servo_init()

{

  // WGM0 0b001 (phase-correct PWM, 8-bit)

  // COM0A 0b10 (clear OCR0A on compare match, set on BOTTOM)

  // COM0B 0b10 (clear OCR0B on compare match, set on BOTTOM)

  // CS0 0b011 (32 prescaler)

  TCCR0A = _BV(WGM00) | _BV(COM0A1) |_BV(COM0B1);

  TCCR0B = _BV(CS01) | _BV(CS00);

  OCR0A = 0;

  OCR0B = 0;

  // set output pins

  DDRD |= _BV(PD5) | _BV(PD6);

  set_servo1(0);

  set_servo2(0);

}

void set_servo1(int8_t angle)

/* angle must be between -128 and 127 */

{

  servo1_angle = angle;

  OCR0A = (uint8_t)(angle + 128);

}

void set_servo2(int8_t angle)

/* angle must be between -128 and 127 */

{

  servo2_angle = angle;

  OCR0B = (uint8_t)(angle + 128);

}

int8_t get_servo1()

{

  return servo1_angle;

}

int8_t get_servo2()

{

  return servo2_angle;

}