El problema que le veo yo a una aproximación tan matemática es que, aunque sobre el papel, puede ser muy exacta, supone una enorme sobrecarga de cálculo para el procesador. Estos microcontroladores son potentes cuando trabajan con números enteros pequeños pero extremadamente ineficientes cuando trabajan con números en coma flotante.
En una aplicación en la que el procesador, además de controlar 4 o 5 servos, tenga que hacer muchas otras cosas a la vez y en tiempo real, este enfoque puede fácilmente desbordar la capacidad de cómputo en coma flotante del controlador.
Una aproximación, menos académica, pero más eficiente con este tipo de controladores es tener las funciones pre-calculadas y almacenadas en una tabla. Como estas tablas son constantes, pueden ser de "solo lectura" y almacenarse en la "memoria de programa", mucho más abundante, en lugar de en la RAM donde habitan las variables.
El proceso de construir el algoritmo de control, en esencia, consiste en "dibujar" la función de respuesta que se quiere.
Una función básica es "estática", esto es, supone que el sistema no tiene inercia, y además, supone que las fuerzas de resistencia son constantes.
En este esquema básico, se mide la posición del servo (y el error) pero no la velocidad con que se mueve el servo (ni la velocidad con que varía el error). Así que el bucle de control está cerrado en un grado (el de la posición) pero abierto en los grados superiores (la velocidad)
De esta forma, damos por hecho que si ordenamos al motor que se mueva a velocidad V, con un digitalWrite(V) el motor pasará instantáneamente a moverse con velocidad V (este bucle está abierto). Esto implica que las fuerzas que puedan estar oponiéndose al movimiento del servo (si ese servo mueve el alerón de un avión, por ejemplo) no son tenidas en cuenta y tampoco se tiene en cuenta la energía cinética que tenga acumulada el sistema: cuando ordenamos al motor que se pare (Velocidad=0) nos conformamos con que los rozamientos pasivos absorban la energía y detengan el sistema un "poco más allá"
Para sistemas pequeños y sencillos, esta primera aproximación puede funcionar. En esta aproximación suponemos que las velocidades del motor serán proporcionales a la energía que le enviemos (bucle abierto) y la energía que enviemos al servo será solo una función del error instantáneo (de la distancia que tiene que moverse hasta anular el error)
Así que podemos "dibujar" una tabla pre-calculada o ajustada mediante tanteos, de energías a enviar al servo en función del error actual. En un sistema pequeño bastan muy pocos valores para definir la forma de esa curva porque las inercias del sistema promedian los "escalones"
Si la posición del joystick y del servo están definidas con 10 bits (1024 valores), el acceso a una tabla de energías-error de 32 valores podría ser así:
byte tabla_energía[32] = {7, 16, 19, 23, ...}
digitalWrite(motor, tabla_energía[error >> 5]);
Para sistemas más grandes, el bucle de la "energía-velocidad" debe cerrarse. No solo hay un error de posición, sino también un error de variación de posición (de velocidad)
La posición, el error actual requiere programar cierta velocidad pero cosas como la inercia, o perturbaciones externas pueden hacer que la velocidad que se consiga no sea "la esperada" y la diferencia es un error de segundo grado, un error de velocidad.
Si miramos al servo a intervalos de tiempo conocidos, la lectura de su potenciómetro nos dice la posición en que se encuentra, si restamos la posición actual de la que tenía la última vez que lo medimos y conocemos cuanto tiempo ha pasado, sabemos también la velocidad.
Esto nos permite compensar fuerzas externas desconocidas (el error de velocidad que vemos nos permite conocerlas) y hacer que el motor frene activamente enviándole energía en "la dirección equivocada"
Para esto podemos usar el método anterior pero con una tabla de dos dimensiones que defina qué energía debe enviarse al servo en función del error actual y de la velocidad a que se está corrigiendo ese error. Una tabla de 16x8 bastaría para casos bastante complicados y una tabla de 32x16 permite controlar el vuelo de un cohete Ariane.
Dibujar estas tablas es una mezcla de cálculo y arte.
Una hoja de cálculo, como OpenOffice, sirve perfectamente y yo creo que un Arduino basta para calcular estos valores en "tiempo diferido"
Luego, los datos provisionales se someten a experimentos: ver si es capaz de frenar en seco o se pasa del semáforo, la aceleración máxima que puede alcanzar pero de forma que el sistema tenga tiempo de frenar luego o si resiste bien a las fuerzas externas.
Como las hojas de cálculo permiten hacer gráficos de estas funciones contenidas en las tablas, y como podemos ver cuando el sistema se excede o se queda corto, con un poco de práctica, el último ajuste puede hacerse puramente a ojo.
Los cohetes que pusieron en órbita a los primeros astronautas, Semiorka y Atlas-Mercury, no es que tuviesen ordenadores incapaces de calcular en coma flotante, es que no tenían ordenador alguno.