Ich würde als erstes mal ausprobieren, ob es überhaupt an der Regelung liegt. Erreicht der Asuro denn noch die volle Geschwindigkeit, wenn du ihn ohne Regeung einfach geradaus fahren lässt?

Wenn ja, würde ich das Programm mal so umschreiben, dass die wichtigen Werte der Regelung aufs Terminal ausgegeben werden, während du den Asuro ein bischen über der Linie bewegst.
Dann müsstest du an den Ausgabewerten wenigstens ungefähr sehen können, wo es klemmt.