Gera, entiendo lo de la matriz, incluso el código que proponia tambien llena una matriz que puede hacerse tambien unitario y luego al momento de aplicarlo lo multiplicamos por un coeficiente. Aunque tu solución lo que calcula es la altura y los puntos actual y nuevo tienen que tener la misma altura.
De todas formas, por el momento son cálculos y especulaciones en el aire, voy a ver si estos días puedo fabricarme una pata y llegar a mover los servos en alguna dirección
Con respecto a tu programa, creo que puedes mejorar incluso un poquito el tiempo de interrupción, yo lo probé con 40us y no me dió problemas, no probé con menos todavia.
Creo que tienes un error en los cálculos de la resolución, porque el pulso alto del servo es 2ms, entonces -> 2ms/50us=40, por lo que si tienes servos de 60° te da una resolución de 1,5°. Mucho mas aceptable de lo que pensabas
Creo que ese valor 20 que calculas lo usas en el programa, quizas debas cambiarlo.
Lo que no veo son los 0,5ms iniciales, o eso ya lo tienes incorporado en el valor del duty de cada servo?
El otro punto que le veo a diferencia de mi programa es que tu siempre mantienes el tiempo de la interrupción en 50us incluso posterior a que terminó el pulso de 2,5ms. Es decir, de 2,5ms hasta 20ms tienes 17,5ms que no es necesario tener interrupciones por el timer.
Lo que yo hago es modificar el valor del timer para que desborde a 17,5ms y así cae la interrupción cuando deben ponerse nuevamente los pines de los servos en alto.
Similar con el 0,5ms iniciales donde no es necesario modificar el estado de los pines.
De todas formas, si así te funciona bien pues no toques nada.
Siempre me ha quedado la duda de que si tener interrupciones afecta en algo al funcionamiento del microcontrolador, mientras que no afecte al programa obvio, me refiero solo al microcontrolador sin tener en cuenta el desarrollo del programa.
Veo que tu comunicacion la atiendes por interrupción, lo has probado ya? tener la interrupcion cada 50us afecta en algo?