
se ve muy suave la respuesta de tu robot.
para el PID lo que hice fue ir incrementando la Kp hasta que empezara a oscilar y posteriormente la ki para suavizar la respuesta, el problema si subes mucho la ki es que el robot tarda mucho en quedarse quieto, que parece que es lo que le da esa pequeña oscilación al tuyo.
Otra cosa que puedes verificar es que tu punto de equilibrio coincida con el setpoint de tu controlador.
Creo que eso seria para hacer un poco mas rápida la estabilización de tu robot, pero me parece muy buena la respuesta que tiene, felicidades