ok, das waren ja eine Menge Tips für Verbesserungen und ich habe natürlich gleich versucht das umzusetzen!

Also der Timer 1 läuft jetzt mit einem Prescaller von 8 (für eine noch bessere Genauigkeit

In der Interruptroutine habe ich nun eine Statemaschine verwendet, das hat den vorteil dass man es im Prinzip beliebig erweitern kann, und da die 20ms sowieso nicht so genau sind, laufen diese einfach nach dem letzen Servo ab, Also der Ablauf ist:
Servo 1 Starten und Timer1 einstellen
Servo 1 Stoppen
Servo 2 Starten und Timer1 einstellen
Servo 2 Stoppen
20ms Warten und zurück zu Servo1

das ganze sieht dann so aus:
Code:
//Globale Variablen:
uint16_t Greifer_Achse1=90;
uint16_t Greifer_Achse2=150;
uint8_t State;

//*** Interrupt von Timer 1 ***

ISR(TIMER1_COMPA_vect)
{

#define Servo1Start 1
#define Servo1Stop 2
#define Servo2Start 3
#define Servo2Stop 4
#define Wait20 5

	if (State == Servo1Start)
	{
		PORTC |= IO_PC5;
		TCNT1 = 0;
		OCR1A = Greifer_Achse1*22+400;
		State ++;
	}
	else if (State == Servo1Stop)
	{
		PORTC &= ~IO_PC5;
		TCNT1= 0;
		OCR1A = 100;
		State ++;
	}
	else if (State == Servo2Start)
	{
		PORTC |= IO_PC3;
		TCNT1 = 0;
		OCR1A = Greifer_Achse2*22+400;
		State ++;
	}
	else if (State == Servo2Stop)
	{
		PORTC &= ~IO_PC3;
		TCNT1 = 0;
		OCR1A = 100;
		State ++;
	}
	else if (State == Wait20)
	{
		TCNT1 = 0;
		OCR1A = 40000;
		State = Servo1Start;
	}
	else
	{
		DDRC |= IO_PC5;		//Ausgang Servo1
		DDRC |= IO_PC3;		//Ausgang Servo2
		State = Servo1Start;
	}
}
Ich glaube damit habe ich eine Minimale Interruptzeit, und muss auch keine Stopwatsches verwenden, d.h. alles läuft quasi im Hintergrund ab.
Ich hoffe ihr hab das so gemeint, allerdigns bin ich zufrieden mit dem Ergebnis, die Servos laufen nun um einiges flüsssiger und der Winkel stimmt noch genauer

Als nächstes werde ich versuchen die überaus "hässlichen" Globalen Variablen loszubekommen, hat dafür vielleicht noch jemand einen Tipp?

Danke an alle die mir dabei geholfen haben
mfg Gerko

EDIT: Eine kleine Änderung habe ich noch vorgenommen, anstadt den Timer andauern zurückzusetzen (mit TCNT1=0) zähle ich einfach immer bei OCR1A den neuen Wert dazu. Das hat den Vorteil, dass man OCR1B noch für andere Funktionen verwenden könnte, außerdem spart man damit wieder 2 Taktzyklen.