Habs als PD-Regler realisert, wie hier beschrieben:

https://www.roboternetz.de/wissen/in...gelungstechnik