Das Problem ist nicht Sensordaten auszulesen oder die entsprechenden Vektoren zu berechnen, sondern die Motorsteuerung

copro_setTargetRel(-tld, trd, speed);
copro_setTargetRel(tl, tr, speed);
copro_setTargetRel(tld, -tlr, speed);

funktioniert so nicht, weil keine Rückmeldung "Aktion abgeschlossen" vom Coproz kommt. Also zB Flag abfragen bevor die nächste Aktion ausgeführt werden kann.

1. Möglichkeit:
Wenn ich zwischen den Befehlen ein Delay 100 msec einfüge, funktioniert es. Ist aber keine schöne Lösung.

2. Möglichkeit (Vorschlag von Hr. Springob)
Ticks-Ist mit Ticks-Soll in einer Schleife vergleichen und weiter wenn (Ticks-Ist + delta) = Ticks-Soll
delta ist notwendig, weil ansonsten evtl. der Sollwert niemals erreicht wird.

Ist schon besser, aber noch nicht optimal. Es dauert evtl. ziemlich lange bis Sollwert erreicht wird.

Hängt vielleicht auch mit den Reglerdaten zusammen (PID)?