#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#define ever (;;)

volatile     uint8_t  servo    = 0x01;
volatile	 uint8_t  index    = 0;
static const uint16_t slot_len = 2500;
volatile     uint16_t servo_pos[8] = 
{
	1000, 1000, 1000, 1000,
	1000, 1000, 1500, 2000
};


ISR(_VECTOR( 5)) // Timer/Counter1 Compare Match A
{
	PORTB &= ~(servo);
	servo  = (servo << 1) | (servo >> 7);
	index  = (index + 1) & 7;
}

ISR(_VECTOR(13)) // Timer/Counter1 Compare Match B
{
	PORTB |= servo;
	OCR1A  = servo_pos[index];
	TCNT1  = 0x0000;
}

int main (void)
{
	PORTB = 0x01;	// Port_B[0]    = high, Port_B[1..7] = low
	DDRB  = 0xFF;	// Port_B[0..7] = output
	
	// Enable both Timer1 Compare Match Interrupts.
	TIMSK = (1 << OCIE1A) | (1 << OCIE1B);	
	
	// Start with first servo;
	OCR1A  = servo_pos[index];
	OCR1B  = slot_len;
	
	TCCR1B = (1 << CS10);
	sei();
	
	for ever; // endless loop
}