Parece que la velocidad del servo no está siendo controlada adecuadamente.

¿Puedes frenar el movimiento del brazo hacia abajo?

Recuerde, para que la cinemática inversa funcione correctamente, necesita un control muy preciso de la velocidad de rotación. Si su sistema tiene el brazo moviéndose de un punto a otro a la velocidad máxima posible, los servos pueden estar alcanzando su velocidad de rotación máxima. Como tal, un servo completaría su movimiento antes que el otro, lo que convertiría su línea en una serie de segmentos de arco, que se mezclan parcialmente.

Intente controlar el sistema para que el brazo se mueva a los puntos en un sistema de coordenadas X-Y. Si el brazo puede mover correctamente el lápiz a los puntos especificados, es un problema de velocidad de servo. Si no, es otra cosa.

Ya que es casi imposible asegurar que dos servos se muevan exactamente a la misma velocidad, la solución simple y global sería generar una trayectoria para sus coordenadas de efector final (X, Y). Entonces, por ejemplo, si desea dibujar una línea recta, simplemente cree una trayectoria lineal de la forma y = mx + b (debería ser familiar para la mayoría de ustedes). Simplemente haga que la x sea una función del tiempo colocando "x = t; t ++;" al final del bucle. Por supuesto, debe tener un tiempo de bucle fijo: si no necesita hacer nada más durante el movimiento, simplemente use un bucle while que termine cuando se alcancen las coordenadas deseadas. Otra ventaja de este método es que puede variar la velocidad a la que dibuja la línea; por ejemplo, en lugar de "t ++" puede poner "t + = 0.7" para una velocidad más lenta.