@malthy
Eigentlich nicht, zumindest noch nicht, denn der Code ist nicht sehr leserlich und daher trau ich mich nicht das eifach so zu Veröffentlichen.
Es müsste noch einiges auskommentiert werden, und dazu habe ich im Moment nicht Zeit.

@Festblatte
Es ginge sogar noch Ruhiger, dazu muss ich aber die Sensorik noch etwas überarbeiten.

@uwente
So wie der Stand im Moment ist würde der Roboter auf einer Gefällstrecke
enifach davonrollen. Die Motoren haben noch keine Decoder, daher hat der
Controller eigentlich keine Ahnung, ob der Roboter nun steht oder fährt.
Alles was ihn interessiert ist, dass er senkrecht steht.
Mit einer Geschwindigkeitsmessung währe es dann schon möglich, dass der
Roboter auch auf einer schiefen Ebene Manövrierfähig ist.
Der Geschwindigkeitsregler würde dann dem jetzt vorhandenen Lageregler
den Sollwert vorgeben.