«

»

Nov 28

Control de un balancín I

Vamos a usar un motor de aeromodelismo para controlar el ángulo de un balancín lastrado en un extremo. El lastre no es suficiente para equilibrar el balancín que tiende a caer hacia el lado del motor, por lo que este debe aportar el empuje restante necesario. El objetivo es dar una aplicación práctica al módulo 10DOF mediante un sistema sencillo, para luego intentar aplicarlo al control de un aeromodelo.

Utilizamos un DSPIC30F4012 que da más que suficiente potencia de cálculo, a la vez de un montón de E/S. De la placa 10DOF usaremos los giróscopos L3G4200D y los acelerómetros ADXL345, para estimar el ángulo de giro. Un variador y un motor brushless con una batería de 11.1V nos dan la potencia suficiente para hacer girar el balancín y un receptor de aeromodelismo modificado para extraer la señal ppm nos permite controlarlo todo con una emisora de 35Mhz.

El primer paso es leer correctamente el ángulo de giro. Pondremos la placa 10DOF de forma que el giro en “Y” coincida con el eje de giro del balancín.

En la horizontal el acelerómetro tendrá una componente en Z igual a 9,8m/s² que para la resolución del ADXL345 corresponde al valor 250, mientras que en Y y X el valor será 0. Al ir girando el balancín aumenta el valor de la aceleración en X y disminuye en Z, hasta el máximo a 90º. El ángulo se obtiene fácilmente como atan2(az,ax) , el módulo debe ser siempre 250 (aproximadamente), salvo que existan aceleraciones diferentes a la de la gravedad. Precisamente para detectar esas aceleraciones, controlamos que el módulo se mantenga en ese valor antes de dar por buena la medida del ángulo.

Las rutinas de comunicación son las siguientes:

#define ADXL345   0xA6 // Acelerómetro

#define acel_x() ((SINT16)sensor_read_reg_alt(ADXL345,0x32))
#define acel_y() ((SINT16)sensor_read_reg_alt(ADXL345,0x34))
#define acel_z() ((SINT16)sensor_read_reg_alt(ADXL345,0x36))

UINT16 sensor_read_reg_alt(UINT8 sensor,UINT8 addr) 
{
UINT8 th,tl;

   i2c_start();
   i2c_write(sensor);
   i2c_write(addr);
   i2c_start();
   i2c_write(sensor|0x01);
   tl=i2c_read(1);
   th=i2c_read(0);
   i2c_stop();
   return make16(th,tl);
}

void ADXL345_init()
{
   // Inicializa
   sensor_write_reg(ADXL345,0x2D,0x00);
   delay_ms(100);
   sensor_write_reg(ADXL345,0x2D,0x10);
   delay_ms(100);
   sensor_write_reg(ADXL345,0x2D,0x08);
   sensor_write_reg(ADXL345,0x31,0x0B);
   sensor_write_reg(ADXL345,0x2C,0x09);
   // Offset de calibración en cada eje
   sensor_write_reg(ADXL345,0x1E,3); 
   sensor_write_reg(ADXL345,0x1F,1);
   sensor_write_reg(ADXL345,0x20,5);

}

Por otro lado disponemos también del giróscopo que nos da las variaciones de la velocidad angular en cada eje, en este caso al ir girando en Y los ejes X y Z deben mantenerse a 0, y el eje Y da la velocidad angular del balancín. Para obtener el ángulo relativo sólo hay que ir integrando las velocidades. Aproximadamente alfa=alfa+vy*dt. Donde dt será el periodo de muestreo. Lo bueno es que los giróscopos tienen mucho menos ruido, lo malo es que sólo dan una medida relativa del ángulo y que a veces tienen algo de deriva, así que optaremos por la opción intermedia, que es usar el acelerómetro para obtener el valor absoluto del ángulo y eliminar la deriva a largo plazo y el giróscopo para sacar los valores instantáneos y reducir el ruido.

#define L3G4200D  0xD0 // Giróscopo 

#define vel_x() ((SINT16)sensor_read_reg_alt(L3G4200D,0xa8))
#define vel_y() ((SINT16)sensor_read_reg_alt(L3G4200D,0xaa))
#define vel_z() ((SINT16)sensor_read_reg_alt(L3G4200D,0xac))

void L3G4200D_init()
{
   delay_ms(100);
   sensor_write_reg(L3G4200D,0x20,0x0f);
   delay_ms(5);
   sensor_write_reg(L3G4200D,0x24,0x00);
}

El cálculo del ángulo con el acelerómetro:

float fax,fay,faz,mod_acc;

   fax=ax;
   fay=ay;
   faz=az;

   mod_acc=fax*fax+fay*fay+faz*faz;
   mod_acc=sqrt(mod_acc);

   alfa_acc=57.295779*atan2(faz,fax)-90.;
   mod_acc*=0.0392266;

   if (alfa_acc>180.) alfa_acc=360-alfa_acc;
   if (alfa_acc<-180.) alfa_acc=360+alfa_acc;

El cálculo con el giróscopo:

   alfa_giro=0;   
   dt=22.95E-3; // Periodo de muestreo en sg.

   d_y=0.0; // deslizamiento en estacionario

   for (i=0;i<100;i++) {
      d_y+=(float)vel_y();
      delay_ms(10);
   }
   d_y/=100.;

   ax = acel_x();
   ay = acel_y();
   az = acel_z();

   calc_acc();

   alfa_giro=alfa_acc; // Ángulo inicial tomado del acelerómetro

...         

         vy = vel_y();

         alfa_giro+=((float)vel_y()-d_y)*dt*250./32768.;

         if (alfa_giro>180.) ang_y=360-alfa_giro;
         if (alfa_giro<-180.) ang_y=360+alfa_giro;

Ya sólo falta controlar continuamente la desviación entre el ángulo medido por el acelerómetro y el giróscopo, para actualizar el valor del ángulo cuando exista un error excesivo.

if ((abs(alfa_giro-alfa_acc)>1) && (abs((float)vy)<20.)) alfa_giro=alfa_acc;

Podríamos añadir también que el módulo de la aceleración sea cercano a “1g” para hacer la actualización si tenemos mucha vibración.  abs(mod_acc-9.8)<1

Vamos a ir adelantando trabajo a la vez que implantamos una base de tiempo para el muestreo de los sensores. En aeromodelismo es normal utilizar un periodo de muestreo de 20 ms que equivale a 50Hz. Montaremos un temporizador (TIMER3)  que lance una interrupción cada 20ms para actualizar la salida del variador del motor y muestrear los sensores.

Hemos elegido el reloj interno del PIC configurado a 58,960 Mhz, el valor de base para los temporizadores de Fcy/4 = 14,740 Mhz. Eligiendo un divisor de 8 nos queda una base de tiempo de 542.74 ns. Para sacar los 20ms necesitamos 36853 pulsos.

La configuración del temporizador es:

setup_timer3(TMR_INTERNAL|TMR_DIV_BY_8,36853);

Que hará saltar una interrupción cada 20ms. Aunque sólo usaremos una salida para el motor vamos a prepararlo para  4. Almacenamos los valores de cada salida en una matriz y aprovechamos la interrupción para generar 4 pwm, uno por salida.

#int_TIMER3
void  TIMER3_isr(void) 
{

   t3=tout[idxout];
   set_timer3(36853-t3);
   switch (idxout) {
      case 0:
         output_high(PIN_B0);
         break;
      case 1:
         output_low(PIN_B0);
         output_high(PIN_B1);
         break;
      case 2:
         output_low(PIN_B1);
         output_high(PIN_B2);
         break;
      case 3:
         output_low(PIN_B2);
         output_high(PIN_B3);
         break;
   }
   sumtout+=t3;
   idxout++;
   if (idxout>3) {
      output_low(PIN_B3);
      set_timer3(sumtout);
      sumtout=0;
      idxout=0;
      flag_tm3=1;
   }      
   numcic++;
}

Ya de paso generamos unos contadores para el muestreo de los sensores.

Por otro lado utilizamos una interrupción externa y un temporizador para muestrear la señal PPM procedente de la emisora, como vimos en la entrada anterior.

#define us_cic(tiempo) ((int32)(tiempo)*10000/5427)
#define cic_us(ciclos) ((int32)(ciclos)*5427/10000)

#int_TIMER2
void  TIMER2_isr(void) 
{
   idx=0;
   failsafe=1;
   sync=0;
}

#int_EXT0
void  EXT0_isr(void) 
{
   t2=get_timer2();
   set_timer2(0);
   if (t2>us_cic(8000)) {
      tsync=t2;
      sync=1;
      idx=0;
      failsafe=0;
      return;      
   }
   if (t2>us_cic(3000) || t2<us_cic(500)) {
      idx=0;
      failsafe=1;
      sync=0;
      return;
   }      
   if (idx<6) tiempo[idx++]=t2;
}

Por hoy nos limitaremos a enviar los datos muestreados 50 veces por segundo al PC para representarlos con un pequeño programa en visual basic. La línea roja es la velocidad angular, la azul el ángulo medido por el acelerómetro, y la verde el estimado con el giróscopo.

 

En el próximo capítulo de esta entrega, intentaremos definir una rutina de control del motor que a partir de estas informaciones sea capaz de estabilizar el balancín en un ángulo proporcional a uno de los canales de la emisora.

Hasta la próxima.

 

5 comentarios

2 pings

Ir al formulario de comentarios

  1. perseo963

    hola..
    estuve leyendo tu código pero no entiendo porque no usas los registros 0x33, 0x35 y 0x37 del acelerometro adxl345

    1. David

      En realidad si que los uso, fíjate en la rutina “sensor_read_reg_alt” Se envía la dirección del primer byte del registro de 16bits (0x32, 0x34, 0x36), pero luego se leen dos bytes, el adxl354 se encarga de autoincrementar la dirección cuando hacemos otra lectura.

      Puedes echar un vistazo al datasheet, al final de la página 18.
      http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf

  2. Miguel Angel

    Muy buenas David!. Me encanta ver como hay gente que hace paginas como estas donde poder explicar las ideas, yo mas que blog tengo canal de youtobe donde tambien tengo tutoriales sobre electronica, cada uno en su medio 😉 Te queria preguntar: ahora mismo me encuentro en la tarea de hacer un quadcopter, programando en microcontroladores asi en plan casero y desde la base de todo, como veo que estas tu desarrollando tambien esto. Pero me surge un gran problema. para medir angulos estando el acelerometro parado es muy facil y perfecto pero al moverse en el quadcopter debido a que esta en moviemiento ya no mide los angulos como deberia y tendria que usar el giroscopo como muy bien explicas. Entonces me he detenido a echarle un vistazo a tu código pero no logro entender del todo la sintaxis, lo de atan2( , ) y todo esto, porque son las coordenadas de un punto?, cosas asi. Me gustaria en cuanto tuvieras ocasión si podrias explicarme exactamente (no mediante código, sino por palabras, los pasos vamos) como realizas la medida del ángulo combinando las dos mediciones del acelerometro y giroscopo y el empleo del filtro este que comentas, como se implementaría. De antemano muchas gracias. Un saludo!

    1. David

      Gracias Miguel Angel. El tema del cuadricoptero tiene asociadas unas matemáticas relativamente complejas, fíjate que aquí sólo hablamos del movimiento en un plano y la cosa ya se complica, imagínate con tres dimensiones 😉
      Como ahora no tengo tiempo de un artículo más extenso, te diré en pocas palabras como va el tema. El acelerómetro es capaz de sentir las aceleraciones a las que está sometido el cuadricóptero. Cuando está estable o parado lo único que nota es la aceleración de la gravedad tirando de el hacía “abajo”, y si el sistema está horizontal esto indica que sólo hay componente en el eje Z (az) que además vale lo mismo que la aceleración de la gravedad g. Cuando estamos en un plano X/Z como en el caso del balancín la otra componente (ax) será cero (bueno la ay también pero nos da igual porque en ese plano no nos movemos). Si el balancín está girado la componente az disminuye y la ax aumenta, hasta que a 90º ax es igual al valor de la gravedad y az es cero.
      De ahí que en un sistema estacionario el ángulo viene dado por la relación de estos valores atan(ax/az), fíjate que esta función tiene un problema, si ax y az son < 0 devuelve el mismo valor que si son positivas y sin embargo el cuadricóptero está cabeza abajo. También hay un problema cuando az=0, ya que el ordenador no puede dividir un número por 0 (y sin embargo la solución es real y vale 90º), por eso se usa la función atan2(ax,az) que funciona igual pero da ángulos correctos en estas situaciones. Otra cosa es que el ángulo se devuelve en radianes y yo quería tenerlo en grados (me parece más visual) por eso multiplico por 57.29...) y lo giro 90º para que me de el ángulo del plano horizontal del cuadricóptero. Además de todo esto sabemos que cuando aplicamos una fuerza sobre un objeto se produce una aceleración que es inversamente proporcional a la masa del objeto y directamente a la fuerza, esa aceleración también la siente el acelerómetro, y hace que cuando el cuadricóptero se mueve, las fuerzas que ejercen los motores, la resistencia del aire y otros eventos desvirtúen la medida del sensor. Pero al menos tenemos una forma de saber si la medida es buena o no, porque el módulo de la aceleración debida a la gravedad es igual a la raiz cuadrada de la suma de las componentes en cada eje al cuadrado e igual a 9.8m/s², así que si el sensor nos da una valor cercano a este sabemos que las otras fuerzas son muy pequeñas o están equilibradas y no afectan a la medida. Sin embargo cuando nos alejamos de ese valor tenemos que recurrir a otro método para estimar el ángulo, o el resultado va a ser totalmente falso. Para eso tenemos el giroscopio que nos da la velocidad angular en cada eje(vx,vy,vz). ¿Qué quiere decir esto? Que si conocemos el ángulo de partida y la velocidad se mantiene constante, el ángulo después de un tiempo será alfa=alfa0+t*v, y aunque la velocidad no sea constante si miramos el sensor cada poco tiempo sólo hay que ir sumando al ángulo el valor v*t en cada instante. ¿Cual es el problema? El giroscopio va sumando errores muy pequeños, pero los suma muchas veces por segundo, por lo que al cabo de un tiempo la medida es muy mala. La solución es usar el acelerómetro cuando sabemos que da valores correctos para estimar el ángulo y sólo usar el giroscopio para arreglar los valores fuera del equilibrio. Al final hay muchos métodos de hacer esto, pero normalmente todos pasan por utilizar dos "pesos" para sumar el ángulo calculado por cada método y variar esos "pesos" en función de la calidad de la medida del acelerómetro para dar prioridad a una u otra, esto lleva luego a cosas como el filtro de Kalman que es una estrategias más o menos elaborada de estimar el ángulo a partir de las medidas parcialmente erróneas de dos o más sensores diferentes. Todo esto vale para un plano, pero se complica un poco al pasar a tres dimensiones, ya que existen correlaciones entre los ejes que requieren usar el cálculo matricial, eso mejor lo dejamos para otro día. Un saludo.

  3. martin

    Hola estimado , estube viendo tu programa , pero cuando programo mi dspic 4012 , con oscilador interno fsr_pll166 y delal clock = 117920000 no funciona nada , sera el compilador ? , vos lo usas con pll8, que puede ser ? Tengo la version ccs 4.114

  1. Control de un balancín II » Blog de inventos

    […] « Control de un balancín I […]

  2. Construyendo un cuadricoptero I » Blog de inventos

    […] Control de un balancín I y II […]

Deja un comentario

Tu dirección de correo electrónico no será publicada.

Puedes usar las siguientes etiquetas y atributos HTML: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <s> <strike> <strong>

Uso de cookies

Este sitio web utiliza cookies para que usted tenga la mejor experiencia de usuario. Si continúa navegando está dando su consentimiento para la aceptación de las mencionadas cookies y la aceptación de nuestra política de cookies, pinche el enlace para mayor información.plugin cookies