Hauptsächlich Formatierungen
findet optimalen input(servo), one step ahead.
wegen zeitproblem noch mit den intgratorparametern rumspielen!