Ich hab leider mein altes Programm nicht mehr, finde aber beim überfliegen deines jetzt nirgends Werte für Kp, Ki und Kd?

Das, was du beschreibst, deutet drauf hin, dass die Regelung viel zu träge reagiert.
Da müsste -erstmal- die Erhöhung von Kp Abhilfe schaffen.

Was aber auch wichtig ist: Tempo. Daher ist es keine so gute Idee, die serielle Konsole zu benutzen- die bremst gewaltig!
Das Ganze muss möglichst schnell laufen, je früher die Regelung auf Abweichungen reagiert, umso besser.