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.

 

6 pings

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

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

Deja una respuesta

Tu email nunca se publicará.

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

ACEPTAR
Aviso de cookies