No entiendo el código muy bien pero la intensidad del ajuste, o del movimiento, debería ser de algún modo proporcional al error que se trata de corregir. El joystick se mueve por su cuenta y el motor debe seguirle y copiar la posición del joystick. Si la posición del motor no corresponde con la del joystick aparece un desfase o error y se trata de corregir ese error.
Por ejemplo: si el joystick está en la posición 650 y el motor en la posición 512 (en el centro), el error, el desfase, que hay que corregir es 138. Si el joystick está en la posición 650 y el motor en la posición 660, el desfase a corregir sería de -10. En este segundo caso, para corregir el desfase, el motor tiene que moverse en la dirección opuesta, ya que el signo es negativo, y además una distancia mucho menor.
La velocidad a que se mueve el motor tiene que ser grande cuando está muy lejos de donde debería estar, cuando el desfase a corregir es elevado, y el motor debe ir reduciendo esa velocidad según se acerque a donde debería estar: según el error a corregir se vaya haciendo cero.
He escrito un ejemplo de código que no funciona directamente sino que sirve para ilustrar el principio.
#define MIN_AJUSTE 50
#define PAR_A 0.25F
#define PAR_B 20
#define DERECHA 1
#define IZQUIERDA 0
void loop(){
int error;
error = leeError();
corrige(error);
}
int leeError(){
int motor, joystick, err;
joystick = analogRead(pot2);
motor = analogRead(A3);
error = joystick – motor;
return(err);
}
void corrige(int err){
byte dir = DERECHA;
int velocidad;
if(err==0) { parada(); return; } // (1)
if(err<0) { dir = IZQUIERDA; err = -err; } // (2)
if(err<MIN_AJUSTE) {parada(); return; } // (3)
velocidad = PAR_A * err; // (4)
velocidad = velocidad + PAR_B;
if(velocidad>255) velocidad = 255;
if(dir == DERECHA) giraDerecha(velocidad);
else giraIzquierda(velocidad);
}
void parada(){
digitalWrite(right,LOW);
digitalWrite(left, LOW);
analogWrite(enable, 0);
}
void giraDerecha(int vel){
digitalWrite(right,LOW);
digitalWrite(left, HIGH);
analogWrite(enable, (byte) vel);
}
void giraIzquierda(int vel){
digitalWrite(right,HIGH);
digitalWrite(left, LOW);
analogWrite(enable, (byte) vel);
}
El código está muy descompuesto en funciones y hay muchas líneas innecesarias pero creo que así se ven mejor lo que hace cada parte.
Supondremos que medimos la posición del joystick y del motor con dos entradas analógicas y que esa posición de cada uno puede ir desde 0 hasta 1023.
leeError() simplemente calcula la distancia o desfase que hay actualmente entre la posición que tiene el motor y la posición que debería tener, que es la misma posición que tenga el joystick.
parada(), giraDerecha() y giraIzquierda(), detienen el motor o lo mueven en un sentido o el otro a cierta velocidad.
corrige(error) es la parte donde realmente se define el comportamiento.
Si el error a corregir es demasiado pequeño, si el motor está lo bastante bien posicionado con respecto al joystick, paramos el motor y damos el ajuste por bueno, incluso aunque no sea un ajuste perfecto. Esto se hace para evitar que el motor se ponga a oscilar a derecha e izquierda tratando de corregir un error muy pequeño.
Hay definir una “banda muerta” en la que el error no se intentará corregir por ser demasiado pequeño (y para evitar el peligro de oscilación). La anchura de esta “banda muerta” la definimos con MIN_AJUSTE. En este ejemplo, si el desfase del motor con respecto al joystick es menor que 50 en cualquier dirección el sistema no intenta corregir el desfase. Las líneas (1) y (3) se encargan de cancelar ajustes demasiado pequeños.
La línea (2) se encarga de determinar en qué dirección debe girar el motor. El código asume en principio que el error tendrá signo positivo y la corrección será a la derecha, en caso de que el error tuviera signo negativo, la línea (2) se encarga de cambiar la dirección.
A partir de la línea (4) definimos cuál será la velocidad de corrección en función de lo lejos que esté el motor de donde debería, en función de lo grande que sea el error. Para calcular una velocidad que sea proporcional al error, multiplicamos el error por un parámetro que hemos llamado PAR_A. En este ejemplo PAR_A es un número en coma flotante de valor 0.25 que hará que si el error es 400 la velocidad sea 100 o si el error ha bajado ya a 80 la velocidad sea 20.
Si hacemos este PAR_A mayor, el sistema tendrá mayor velocidad de respuesta y el motor seguirá al joystick con reflejos más rápidos pero existe el peligro de que el motor no pueda luego frenar a tiempo. Cuando el motor se lanza a corregir el error, la inercia hace que el mecanismo acumule energía mecánica que hay que disipar para que el motor se pare. Este PAR_A hay que ajustarlo con tanteos al mismo tiempo que MIN_AJUSTE. Si le pedimos al motor que sea rápido debemos dejarle luego una “banda muerta” más ancha donde pueda frenar. Si movemos el motor muy despacio el ajuste final puede ser más fino.
Tenemos luego el parámetro PAR_B que es algo así como una “potencia mínima”. Esto se pone para vencer rozamientos que pueda haber. Si no lo ponemos, tenemos el peligro de que el motor se pare antes de haber corregido el error por sus rozamientos mecánicos. La función giraDerecha() podría estar ordenándole al motor que se mueva a una velocidad 12 y el motor ser simplemente incapaz de moverse con tan poca potencia, así que añadimos esta potencia mínima para ayudarle.
MIN_AJUSTE, PAR_A y PAR_B hay que ajustarlos a base de experimentos, comenzando por una banda muerta grande y velocidades pequeñas.
Lo que sea derecha o izquierda con relación a lo que sea un signo + o - en el error es arbitrario y el motor podría tratar de corregir el error en la dirección equivocada. En ese caso hay que invertir la polaridad del motor o los potenciómetros o cambiar el significado de derecha o izquierda.
Este código de ejemplo, tal como está escrito, es muy poco eficiente por muchos motivos pero sobre todo por esa multiplicación en coma flotante que es muy costosa para el controlador. En un código más realista emplearíamos una “multiplicación entera” y PAR_A sería un número entero.
La línea (4) y siguientes tendrían un aspecto del estilo de:
#define PAR_A 16
…
velocidad = ((err * PAR_A) >> 6) + PAR_B;
if(velocidad>255) velocidad = 255;