/*
 * $Id: main.c,v 1.11 2005/03/22 19:12:14 bsd Exp $
 *
 */

/*
 * SERVO test program for the BDMICRO MAVRIC series board
 *
 * See: http://www.bdmicro.com/
 *
 * Command 6 servos, use the first UART (UART0) to accept simple
 * commands from the serial port to display and set the servo
 * positions.  Wire the servo signal pins to PORTE3, PORTE4, PORTE5,
 * PORTB5, PORTB6, and PORTB7.  These drive servos SERVO1, SERVO2,
 * SERVO3, SERVO4, SERVO5, and SERVO6 respectively.  Be sure and use a
 * seperate power supply for the MAVRIC board from the servo power,
 * otherwise the power supply may brown-out due to the heavy current
 * draw of the servos and cause the CPU to reset.  Also, be sure and
 * use a good power supply for the servos because they tend to be
 * jittery if their power supply is not strong enough.
 *
 * For each servo, the pulse width is set as follows corresponding to
 * the output compare register:
 * 
 *     Pulse Width    OCR3x/OCR1x
 *       1.0 ms         2000
 *       1.5 ms         3000
 *       2.0 ms         4000
 *
 * The nominal servo center position is 1.5 ms.  This is achieved by
 * setting up timers 1 and 3 to run in normal mode with a prescale of
 * 8 and generate an interrupt on overflow, which at 16 MHz external
 * clock, interrupts about 30 times per second - standard servos want
 * the pulse repeated 20 - 50 times per second, so the prescale was
 * chosen to be within this range so that the timer overflows
 * approximately 30 times per second.  When the timer overflow occurs,
 * the servo signal pin is set to high by the time overflow interrupt
 * handler.  This starts the servo pulse.  When the output compare
 * match occurs as defined by the current time value and the content
 * of the servo's output compare register (one for each servo), then
 * the hardware automatically sets the servo signal pin low precisely
 * ending the servo pulse.  The timer continues to count until
 * overflow occurs and the cycle repeats.
 *
 * This all happens in the background - once configured, your program
 * never needs to initiate a pulse.  To change a servo's position,
 * just assign a new value to that servo's output compare register.
 * For convenience, these are alias'd to the names SERVO1 ... SERVO6.
 *
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/signal.h>

#include <stdio.h>
#include <ctype.h>
#include <stdlib.h>
#include <string.h>

#define SERVO1   OCR3A
#define SERVO2   OCR3B
#define SERVO3   OCR3C

#define SERVO_MIN 2000
#define SERVO_MID 3000
#define SERVO_MAX 4000

//Macro to prevent input going under min or over max
#define SERVO_CLIP(x) (x<SERVO_MIN ? SERVO_MIN : (x>SERVO_MAX ? SERVO_MAX : x))

//Macro to implement the centering-at-2000 behavior
#define OFFSET(x) (SERVO_CLIP(SERVO_MID + x))

/* SIG_OVERFLOW3 - this interrupt handler starts the pulse for servos
 * 1, 2, & 3; the timer 3 output compare function automatically ends
 * the pulse precisely as specified by the OCR3x register which
 * represents the servo position */
SIGNAL(SIG_OVERFLOW3)
{
  TCNT3   = 0;

  /* configure to set outputs on compare match so we can turn on the
   * pulse in the next statement */
  TCCR3A |= _BV(COM3A1)|_BV(COM3A0)|_BV(COM3B1)|_BV(COM3B0)|_BV(COM3C1)|_BV(COM3C0);

  /* force compare match to set outputs */
  TCCR3C |= _BV(FOC3A)|_BV(FOC3B)|_BV(FOC3C);

  /* configure to clear outputs on compare match so that the output
   * compare function ends the pulse */
  TCCR3A &= ~(_BV(COM3A0)|_BV(COM3B0)|_BV(COM3C0));
}

void servo(int16_t s1, int16_t s2, int16_t s3)
{
  SERVO1 = OFFSET(s1);
  SERVO2 = OFFSET(s2);
  SERVO3 = OFFSET(s3);
}

void init_servos(void)
{
  /* Use Timers 1 and 3 to generate the pulses for 6 R/C servos; each
   * timer can do up to 3 servos. */
  
  /*
   * configure OC3A for mode 0: normal, top=0xffff prescale=8 (f~=30):
   *
   * WGM33=0, WGM23=0, WGM13=0, WGM03=0, CS32=0, CS31=1, CS30=0 
   */
  DDRE   |= _BV(PORTE3) | _BV(PORTE4) | _BV(PORTE5);
  TCCR3A &= ~(_BV(WGM31) | _BV(WGM30) | _BV(COM3A1) | _BV(COM3B1) | _BV(COM3C1));
  TCCR3A |= _BV(COM3A0) | _BV(COM3B0) | _BV(COM3C0);
  TCCR3B &= ~(_BV(WGM33) | _BV(WGM32) | _BV(CS32) | _BV(CS30));
  TCCR3B |= _BV(CS31);
  TCNT3   = 0;
  TCCR3C |= _BV(FOC3A) | _BV(FOC3B) | _BV(FOC3C);
  ETIMSK |= _BV(TOIE3);

  /* set all servos to their center positions */
  SERVO1 = SERVO_MID;
  SERVO2 = SERVO_MID;
  SERVO3 = SERVO_MID;
}


int main(void)
{
  /* initialize servos */
  init_servos();

  /* enable interrupts */
  sei();
  
  /* loop */
  while (1) {
    //Control by calling servo() function
	//Or by assigning different values to SERVO1, SERVO2, SERVO3
	SERVO1 = 2500;
  }

}