// Festlegung der Quarzfrequenz
#ifndef F_CPU                   // optional definieren
#define F_CPU 12288000UL        // ATmega 328 mit 12,288 MHz Quarz
#endif

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

//
// Der Prescaler muss so gewählt werden, dass der Ausdruck
// für MILLISEC_BASE einen Wert kleiner als 1024 ergibt
// MILLISEC_BASE ist der Timerwert, der 1 Millisekunde Zeitdauer ergeben
// soll.
//
#define PRESCALER      1024
#define PRESCALER_BITS (1 << CS02)|(1 << CS00)

#define MILLISEC_BASE  ( F_CPU / PRESCALER / 1000 )
#define CENTER         ( MILLISEC_BASE / 2 )

//
// Konfiguration der Servoleitungen
//
#define NR_SERVOS      2
#define SERVO_DDR      DDRD
#define SERVO_PORT     PORTD
uint8_t ServoPuls[NR_SERVOS] = { 1<<PORTD0, 1<<PORTD1};
//
// Werte für die Servoposition
// Gültige Werte laufen von 0 bis 2 * CENTER
// 0           ... ganz links
// CENTER      ... Mittelstellung
// 2 * CENTER  ... ganz rechts
//
volatile uint8_t ServoValue[NR_SERVOS];

ISR (TIMER0_COMPA_vect)
{
	static uint8_t ServoId = 0;

	//
	// den Puls des aktuellen Servos beenden
	//
	SERVO_PORT &= ~ServoPuls[ServoId];

	//
	// welches ist das nächste aktuelle Servo?
	//
	if( ++ServoId >= NR_SERVOS )
	ServoId = 0;

	//
	// die Ausgangsleitung fuer dieses Servo auf 1; den Puls beginnen
	//
	SERVO_PORT |= ServoPuls[ServoId];

	//
	// den Timer so einstellen, dass bei Pulsende, die ISR erneut aufgerufen wird
	//
	OCR1A = MILLISEC_BASE + ServoValue[ServoId];
}

void InitServo()
{
	uint8_t i;

	//
	// Die Servoleitungen auf Ausgang stellen
	//
	SERVO_DDR = ServoPuls[0] | ServoPuls[1] | ServoPuls[2] | ServoPuls[3] |
	ServoPuls[4] | ServoPuls[5] | ServoPuls[6] | ServoPuls[7];

	//
	// Alle Servos in Mittelstellung
	//
	for( i = 0; i < NR_SERVOS; ++i )
	ServoValue[i] = CENTER;

	//
	// Timer auf CTC Modus konfigurieren
	//
	// initialize counter
	TCNT1 = 0;
	OCR1A = MILLISEC_BASE + ServoValue[0];
	TIMSK0 |= (1<<OCIE1A);
	TCCR1B = (1<<WGM12) | PRESCALER_BITS;  // CTC mode
}

int main(void)
{
	InitServo();

	sei();

	_delay_ms( 1000 );

	//
	// testweise einfach alle 8 Servos ansteuern
	// jedes Servo soll sich unterschiedlich schnell bewegen
	//
	while( 1 ) {


		ServoValue[0] += 2;
		if( ServoValue[0] > 2*CENTER )
		ServoValue[0] -= 2*CENTER;

		ServoValue[1] += 1;
		if( ServoValue[1] > 2*CENTER )
		ServoValue[1] -= 2*CENTER;

		_delay_ms( 40 );
	}
}