Die Servoansteuerung sollte normal fast komplett in den Interrupt kommen. So kriegt man ein exactes Timing und hat vor allem noch viel Rechenzeit über. Ich hab es noch nicht Porgrammiert / compiliert, aber ich würde den Code von der Länge auf vielleicht 1 kByte im Flash schätzen. Den Code für die Ansteuerung mehrerer Servos sollte man irgendwo finden - dsa muß man nicht komplett selber schreiben.
Ein Ansatz wäre z.B. der Code im Wiki Bereich unter Servoansteuerung. Da sollte sich aber irgendwo auch noch was bessere Finden. Wichtig für einen kurzen Code ist es z.B. Fleißkommazahlen zu vermeiden und bei nicht zu viele Variablentypen unnötig zu mischen.