Go Down

Topic: Uso de motor DC como cuasi Servo, proyecto avanzado, para sugerencias (Read 2095 times) previous topic - next topic

Selda81

Gente,

En el proyecto que estoy desarrollando utilice este moto reductor como un cuasi servo, le instale un potenciometro de 100 k, para que me sirviera como "encoder" y saber su posición, la idea es usarlo como dirección, moviendolo hacia la izquierda o derecha hasta cierto ángulo, teniendo como referencia la lectura del potenciometro instalado. estoy utilizando un L293D (puente H) para el sentido de giro y PWM.

Este es:


Este es el código que utilizo, simplemente gira a la izquierda o a la derecha sin pasar los límites establecidos (valor del pot) es muy sencillo y funcionó, pero requiero hacerlo mejor, ya que debí colocarle una duración de 1 seg, ya que se estaba pasando demasiado de los límites.

La consulta es que si con PID puedo mejorar el control, alguien que me comparta su experiencia.

Code: [Select]
//Programa para control de carro v.1.1, dirección 
//Selwins Maturana Oct 2012 

int pot2 = A5; //joystick dirección   
int enable = 3; //Enable IC L293D - PWM control de velocidad 
  int right = 9; // Señal para ir hacia la derecha
int left = 7; // Señal para ir hacia la izquierda
int LED = 13; //verificación 
int sign;// Señal potenciometro moto reductor

void setup(){ 
      pinMode(enable, OUTPUT); 
     pinMode(forward, OUTPUT); 
      pinMode(backward, OUTPUT); 
     Serial.begin(9600); 

void loop(){ 

      dir = analogRead(pot2);  // joystick
     sign = analogRead(A3);  //Posición del moto reductor
     sign = map(sign, 0,1023, 0, 255); 
     Serial.println(sign); 
     delay(1000);   

     if(dir >= 344 && sign > 105) { 
         digitalWrite(left, LOW); 
         analogWrite(enable, 70); // Para que gire despacio
         digitalWrite(right, HIGH); 
         delay(1000); // pausa para que no se pase
         digitalWrite(right, LOW); 
         delay(10); 
           } 
           else if (dir <= 334 && sign < 127){ 
              digitalWrite(right, LOW); 
              analogWrite(enable, 80); 
              digitalWrite(left, HIGH); 
              delay(1000); 
              digitalWrite(left, LOW); 
              delay(10); 
           } 
             else 
             {                 
             digitalWrite(right,LOW); 
             digitalWrite(left, LOW); 
             analogWrite(enable, 0); 
             } 
  } 

Mitxel

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.

Code: [Select]
#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;

Selda81


Muchas gracias Mitxel por tu orientación, si puedes revisar lo que avance utilizando PID, agradecería enormemente tu opiníón y consejos:

http://disquegeek.blogspot.com/2012/11/progreso-01-05-nov-mejorando-la.html

Slds

Mitxel

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í:

Code: [Select]

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.




Go Up