Servus,
ich habe mir den Code jetzt nicht angeschaut, aber ihr wisst dass es sich um eine regelungstechnische Aufgabe handelt?!
Stichwort "Inverses Pendel".