//********************************************
#include <avr/io.h
#include <avr/interrupt.h>
#include <util/delay.h>
//********************************************
#define SETSERVO(channel, value)   \
         switch (channel)          \
		 {                         \
		    case 0:                \
				OCR1A = value;     \
				break;             \
		    case 1:                \
				OCR1B = value;     \
				break;             \
		    case 2:                \
				OCR1C = value;     \
				break;             \
		    case 3:                \
				OCR3A = value;     \
				break;             \
		    case 4:                \
				OCR3B = value;     \
				break;             \
		    case 5:                \
				OCR3C = value;     \
				break;             \
		 }

#define PWM_TOP      40000
#define SERVO_REST   3200
//********************************************

void InitalizeServos(void);

int main (void)
{
	InitalizeServos();
	
	for(;;)
	{
		SETSERVO(0, SERVO_REST + 4);
		SETSERVO(4, SERVO_REST - 40);
		SETSERVO(3, SERVO_REST + 12);
		
		_delay_ms(100);
		_delay_ms(100);
		_delay_ms(100);
		_delay_ms(100);

		SETSERVO(0, SERVO_REST - 50);
		SETSERVO(4, SERVO_REST + 60);
		SETSERVO(3, SERVO_REST - 14);

		_delay_ms(100);
		_delay_ms(100);
		_delay_ms(100);
		_delay_ms(100);
	}
}


void InitalizeServos(void)
{
   // PWM mode 14 (fast PWM, TOP=ICR1)
   TCCR1A = (1 << WGM11);
   TCCR1B = ((1 << WGM13) | (1 << WGM12));
   TCCR3A = TCCR1A;
   TCCR3B = TCCR1B;
   
   // Start timers (prescale=8)
   TCCR1B |= (1 << CS11);
   TCCR3B |= (1 << CS11);
   
   // Clear OC1A/OC1B/OC1C pins on each timer
   TCCR1A |= ((1 << COM1A1) | (1 << COM1B1) | (1 << COM1C1));
   TCCR3A |= ((1 << COM3A1) | (1 << COM3B1) | (1 << COM3C1));
   
   // Set register values
   ICR1   = PWM_TOP;     // => 50Hz (T=20ms)
   ICR3   = PWM_TOP;     // => 50Hz (T=20ms)

   OCR1A  = SERVO_REST;  // => 1,5ms
   OCR1B  = SERVO_REST;  // => 1,5ms
   OCR1C  = SERVO_REST;  // => 1,5ms
   OCR3A  = SERVO_REST;  // => 1,5ms
   OCR3B  = SERVO_REST;  // => 1,5ms
   OCR3C  = SERVO_REST;  // => 1,5ms
   
   // OC1A/OC1B/OC1C pins as outputs
   DDRB |= ((1 << 5) | (1 << 6) | (1 << 7));

   // OC3A/OC3B/OC3C pins as outputs
   DDRE |= ((1 << 3) | (1 << 4) | (1 << 5));
}