@muraad:
Danke für den Tip, ich kann leder kein C, aber es scheint so oder so eine relativ komplizierte Rechung zu sein mit zu vielen floats.

http://www.mc-project.de/Pages/Robotik/pi-regler.c

MFG Moritz