Also du musst schon zwei Geschwindigkeiten miteinander vergleichen. Deine Istwerte würde ich vielleicht noch ein bisschen filtern, die sehen sehr verrauscht aus. Jedenfalls müsstest du deine Istwerte in mm/s umrechnen (geht meist über die Anordnung des Sensors) und dein Fehler wäre dann einfach e=Sollwert-Istwert. Musst eben nur drauf achten, dass die Umrechnung stimmt, dass es auch wirklich mm/s sind und nicht Löcher/Zeitschritt oder sowas (es sei denn, daraus ergibt sich direkt mm/s, was aber schon was von Zufall hätte).
Wenn ich das richtig sehe, hast du für den Integratoranteil entweder den Verstärkungsfaktor Kp/Tn oder Ki, was deine Werte da sein sollen, weiß ich nicht, ist ohne den Zusammenhang im Code aber auch schwer zu sagen.