Spiel mit den Werten Kp,Ki und Kd.
Nicht so an der Motorsteuerung herum pfuschen!

Ich hab das damals so gemacht,dass ich mir drei Potis vorübergehend eingebaut hab, je einen für die drei Parameter.
Kann man einfach mal an analoge Eingänge stecken...
So kann man zur Laufzeit herumprobieren. Die jeweils eingestellten Werte hab ich dann auf Display ausgegeben, um auch sehen zu können, in welchen Bereichen ich mich da bewege.

Stell zuerst Kp mal so ein, dass es schon "beinahe hinhaut"- komplett klappen wird das mit Kp alleine nicht!
Ki und Kd auf 0.
Wenn du ein leichtes Übersteuern der Regelung bemerkst, lass Kp so und mach mal vorsichtig mit Ki weiter...mit beiden zusammen sollte das schon "fast funktionieren".
Kd ist dann gewissermassen Feintuning.
Da die drei Werte sich gegenseitig beeinflussen, ist da einiges an Spielerei nötig, darum die Potis- sonst dauert das Wochen (wenn man keine Ahnung hat, in welchen Bereichen die drei sich überhaupt bewegen sollten).