@inka:
Da gibt es einige Möglichkeiten.

Am Einfachsten:
In der while(1)-Schleife gibt es eine Stopwatch-Abfrage (a-la: if (getStopwatch2() > nnn) ...). Darin änderst du dann die 3 Servopositionen.
Das kann man z.B. so machen, dass man die Positionen in einem Array ablegt. In deinem Fall wäre das z.B. ein Array mit 3 Werten je Position. Die Größe (Zahl der Positionen) hängt von der Länge der Bewegung ab, die du machen willst.
Jedesmal, wenn die Stopwatch-Abfrage aufgerufen wird (also alle nnn ms) übergibst du die Werte aus dem Array an die Variablen servoX_position. Dann zählst du noch den Index des Array um 1 hoch.
Dadurch kannst du jede komplexe Bewegung der 3 Servos abspulen.

Wenn das alles in deinem Programm noch besser wird, gibt es mehrere Bewegungsabläufe als Funktionen in der while(1)-Schleife:
Code:
while(true)  
{
	task_SERVO();	// The main servo task
	Bewegung1();
	Bewegung2();
}
Jede Bewegung sieht so ähnlich aus:
Code:
void BewegungX(void)
{
	if (getStopwatchY() > nnn) {	// Change position every nnn ms
		// Hole 3 Servopositionen aus dem Array
		// Lege sie in 3 servoX_position ab
		// Inkrementiere den Array-Index
		setStopwatchY(0);
	}
}
Gruß Dirk