ich arbeite gerade an etwas ähnlichem und gehe folgendermassen vor:
alle 20ms (timer-interrupt) werden alle servo ports eingeschalten. nach einer weiteren ms wird in eine blockierende funktion gesprungen, die die servo-pins wieder abschaltet, in etwa so (opcode):
Code:
for(i=0;i<255;i++) // schleife dauert max. 1ms
{
  if(i>servoposition1)SERVOPORTn=0;
  else warte_solang_wie_wenn_bedingung_erfüllt();
  if(i>servoposition2)..
  ...
}
je nach servo-anzahl wird die auflösung natürlich geringer, bei mehr als 12 Servos sollte man evtl. auf 2 funktionen aufsplitten, die abwechselnd alle 10ms aufgerufen werden.

da die funktion zeitkritisch ist, schreibt man sie am besten in assembler. den rest der zeit (18-19ms) hat man für andere berechnungen frei.

gruesse von der katz