So wie ich das verstehe funktionieren deine Servos schon, kannst sie aber nicht genau ansteuern.
Ich weis nicht ob das weiter hilft, aber warum baust du nicht ein encoder Rad an jedes servo. Dann wüsstest du wo es sich genau befindet (den Winkel).

Kannst du mir bitte hier https://www.roboternetz.de/phpBB2/viewtopic.php?t=27812
sagen wie du die Servos ansteuerst? Ich kriege das irgendwie nicht hin.