Software

El software consta de dos partes totalmente desacopladas, una es el control de lazo cerrado a corto plazo el cuál controla los 3 grados de libertad (roll, pitch y yaw), y la otra es el software de la PC que establece el control de lazo cerrado a largo plazo que controla los otros 3 grados de libertad (y, x y z).

Firmware ARM

El software de control es del tipo secuencial, en lenguaje ANSI-C, haciendo uso de los periféricos y características que ofrece el uC a través de la placa multipropósito, como timers, UARTs, PWMs, detección de flancos, entre otros. El siguiente diagrama de flujo simplificado da una noción del flujo seguido por el software.

Diagrama-de-flujo.dot.png

Como vemos consta de dos tareas principales, una es la interfaz con el mundo exterior. En la cuál los datos pueden ser parámetros en ese instante o establecer alguno de ellos para realizar el control del mismo.

La comunicación es realizada a través de la UART, se estableció un protocolo para intercambiar con la PC. Luego de varias fallas, se decidió implementar una comprobación de errores optando por la comprobación de redundancia cíclica(CRC). Tiene dos modos de envíos de parámetros, uno síncrono y otro asíncrono.

El modo síncrono es desde el uC hacia la PC, cada vez que obtiene datos de los sensores son enviados inmediatamente.
El modo asíncrono es desde la PC hacia el uC. La PC cada cierto tiempo le envía un dato solicitándole un parámetro específico, el uC envía el parámetro que tiene en buffer.

Todo el firmware para el QA3 fue desarrollado con herramientas libres. Como editor de texto no se utilizó entorno gráfico alguno, solamente el editor Vim. Como toolchain de compilación, se utilizó gcc cross-compiled para ARM. Para grabar el microcontrolador se utilizó lpc21isp y por último como herramienta de debug se utilizó insight con la interfaz JTAG.

Los 2 algoritmos fundamentales del firmware son el filtro de estimación del ángulo y el controlador PID. Las partes principales se muestran a continuación.

Código del filtro complementario

   1 void filtro_complementario(void) {
   2     //Cálculo de Phi
   3     angulo.Phi_est = 0.9981 * (angulo.Phi_ant) + 0.005 * velocidad.Phi + 0.0019 * angulo.Phi_med;
   4     angulo.Phi_ant = angulo.Phi_est;
   5     angulo.Phi = angulo.Phi_est - angulo.Phi_zero;
   6     //Cálculo de Theta
   7     angulo.Theta_est = 0.9981 * angulo.Theta_ant + 0.005 * velocidad.Theta + 0.0019 * angulo.Theta_med;
   8     angulo.Theta_ant = angulo.Theta_est;
   9     angulo.Theta = angulo.Theta_est - angulo.Theta_zero;
  10 }

filtro_complementario.c

Código del controlador PID

   1 void pid_update(pid_type* pid){
   2     pid->error = pid->in - pid->ref;
   3     pid->proportional = pid->Kp * (pid->error - pid->error_old1);
   4     pid->integral = pid->Ki * pid->error;
   5     pid->derivative = pid->Kd * (pid->in - 2*pid->in_old1 + pid->in_old2);
   6 
   7     if(pid->in >= 5 || pid->in <= -5){
   8         pid->integral = 0.1 * pid->integral;
   9     }
  10     pid->out = pid->out_old + pid->proportional + pid->derivative + pid->integral;
  11     pid->in_old2 = pid->in_old1;
  12     pid->in_old1 = pid->in;
  13     pid->error_old1 = pid->error;
  14     pid->out_old = pid->out;
  15 }

controlador_PID.c

Software de control de la PC

El software de la PC está realizado en python y se comunica por medio de una conexión socket tcp a través de la placa inalámbrica. Se encarga de tomar las lecturas del joystick, procesarlas y luego enviarlas al uC. También este programa lleva una bitácora con los tiempos de vuelo y datos obtenidos en ese momento en un período de 100ms cada muestra, definiendo la actitud del robot en espacio-tiempo. Para luego ser utilizado en corrección de errores, calibración de las mediciones realizadas, ajustes del controlador, análisis de vibraciones, etc.

También permite establecer los parámetros del control, tanto como los ceros los ángulos

$$\Phi_{0}$

$$\Theta_{0}$

$$\Psi_{0}$

, como así también los valores de

$$K_{p}$

$$K_{i}$

$$K_{d}$

de los PIDs.

main.py.png

Utilidades adicionales

Se generaron herramientas que permitieron el testeo y depuración del sistema.

Por ejemplo en el momento de análisis de vibraciones, se generaron gráficos de distintos instantes de tiempo y su transformada rápida de Fourier. Permitiendo así obtener las distintas respuestas al impulso hasta lograr un valor óptimo.

También se realizo una interface entre la PC y la balanza usada para medir el empuje de los motores.

Todas estas herramientas para analizar los datos obtenidos se escribieron en lenguaje python.