good response with Kp
Kd seems to improve the performance
new calibration