Hallo Charly

Du machst es etwas kompliziert und ...
warum konfigurierst du Pinc.1 als Output und verwendest dann für Servo2 Portc.2?
Wo sorgst du dafür das die Pulselängen erzeugt werden und alle 20ms wiederholt werden?

Ich würde an deiner Stelle einen Timer verwenden der alle 20ms ein Interrupt auslöst. Die entsprechende Interrupt-Service-Routine würde dann an Portsc.0 und Portc.1 einen Low-Wert anlegen. dann würde ich die beiden Ports nacheinander bedienen und die Impulslänge dadurch erreichen das ich mit einem Counter die Länge gemäß ehaltenen Parameter erzeuge. D.h. Portc.x auf High setzen, Counter starten so dass Null durchlauf der gewünschten Länge entspricht und den Port.x Wert wieder auf Low setzen. Dieses jeweils und nacheinader für alle angeschlossenen Servos. NachAbschluß RTI und auf den nächsten 20ms Zyklus warten!