Spontane Idee: Nimm eine Kurve auf, Position/Grad, stelle eine Funktion auf die diese beschreibt. Diese Korrekturfunktion rufst du immer auf, bevor du ein Servo ansteuerst.

Einfachere Lösung: Erst mal - falls vorhanden - Signal im Oszi anschauen - wenn das linear ist: Servo zurückschicken.