Hallo,

Du rufst Deine Funktion auf bevor Du überhaupt die Main Schleife ausführst!
Du hast da immer noch einen Drehwinkel von 1 drin - das funktioniert nicht mit diesem Antriebssystem bzw. mit der RP6Lib.
Und es muss immer task_RP6System in regelmäßigen Abständen aufgerufen werden!

MfG,
SlyD

PS: Schau Dir nochmal die Beispielprogramme in Ruhe an!

PPS:
Das Selftest Programm hast Du komplett durchlaufen lassen?
Bzw. die Drehgeber kontrolliert?