/* 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
 *
 */
#include <avr/io.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))

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

void init_servo_pwm(void) {

	// set up timer3 (16 bit) to act as a triple channel PWM generator
	// we want OC3A, B, and C to be set on reaching BOTTOM, clear on reaching
	// compare match, use ICR1 as TOP and have a prescale of 8 (clock goes to 2Mhz).
	TCCR3A = _BV(COM3A1) // set OC3A/B/C at TOP
		   | _BV(COM3B1) // clear OC1A/B/C when match
		   | _BV(COM3C1) 
		   | _BV(WGM11); // mode 14 (E) (fast PWM, clear TCNT1 on match ICR1)
		   
	TCCR3B = _BV(WGM33)
		   | _BV(WGM32)
		   | _BV(CS31); // timer uses main system clock with 1/8 prescale
		   
	ICR3 = 40000; // used for TOP, makes for 50 hz PWM @ 2MHz clock
	
	
	
	OCR3A = SERVO_MID; // servo at center
	OCR3B = SERVO_MID; // servo at center
	OCR3C = SERVO_MID;
	
	//Note that for the Mavric II B board, OC3A/B/C are at pins 3,4,5 of Port E
	DDRE   |= _BV(PORTE3) | _BV(PORTE4) | _BV(PORTE5);
}

int main(void)
{
  /* initialize servos */
  init_servo_pwm();
  
  //Once initialized, should change servo control by changing 

  /* loop */
  while (1) {
	OCR3A = SERVO_MAX; 
	OCR3B = SERVO_MAX; 
	OCR3C = SERVO_MAX;
	
    //Control by calling servo() function
	//Or by assigning different values to SERVO1, SERVO2, SERVO3
  }

}