Continuamos con la guía, pasos 6 a 10.
Ya lo hemos venido haciendo de forma individual para cada subsistema como parte de su integración propia al Arduino, pero ahora es el momento de considerar el sistema de nuestra aeronave completo con respecto a sus conexiones y el montaje de todos los componentes sobre el marco.
En la figura 6 se ilustra de manera esquemática la totalidad de las conexiones de nuestra aeronave, incluyendo los elementos que ya hemos visto (IMU, receptor de comunicaciones y motores / ESCs) más la batería y algunos elementos adicionales como un par de resistencias para un divisor de voltaje y un LED de indicación de estado. A continuación resumimos los puntos más importantes respecto a este esquema:
Mira acá el video correspondiente a este paso del tutorial.
Ya conocemos de manera más detallada todos los elementos de nuestra aeronave. Ya sabemos cómo funciona cada uno y cómo se integran al Arduino, el cerebro de nuestro dron. Podemos adquirir los datos de los sensores (la IMU), los comandos a efectuar (del control remoto) y operar los motores. Ahora viene un paso muy importante: integrar toda esta información para generar el sistema de control de nuestra aeronave.
Existen muchas y muy diferentes técnicas para diseñar e implementar una estrategia de control para un sistema dinámico, como es el caso de nuestra aeronave. En este tutorial usaremos uno de los métodos más populares, conocido como control PID. La teoría de este tipo de controlador es muy interesante y no demasiado compleja; existen infinidades de páginas donde el lector puede consultar al respecto. Esta es una de tantas páginas donde se explican los principios del controlador PID de una manera sencilla: ver enlace.
Una vez asimilada la información, podemos plasmar el esquema de control en lazo cerrado de nuestra aeronave, como se muestra en la figura 7.1. Como entrada externa tenemos las señales enviadas con nuestro transmisor remoto. El controlador PID, implementado en el Arduino, se encarga de tomar estas señales, o más exactamente, la diferencia entre el estado deseado y el estado real de nuestro dron y calcular la señal de salida adecuada para ser efectuada por el mismo. Ahora bien, debemos distinguir dos elementos claves en nuestro sistema: los actuadores, que son quienes reciben esta señal de control y ACTÚAN, como su nombre lo indica, sobre el sistema. Y los sensores, en este caso la IMU, quienes miden el estado actual del sistema y realimentan esta información al sistema de control. Es por eso que se dice que el sistema es de lazo cerrado, pues la salida del sistema completo es realimentada hacia la entrada continuamente.
Antes de proceder con el código es necesario tener claro a nivel conceptual cómo será nuestro sistema de control y su interacción con los demás componentes de nuestro sistema. Pero, antes de ello, es necesario, si no se ha hecho ya, establecer algunas convenciones básicas sobre el movimiento de nuestro dron. En particular debemos tener claro:
La figura 7.2 nos presenta el diagrama que representa las convenciones que elegiremos para este proyecto. Valga decir que este sistema no es único, el lector es libre de elegir el sistema de coordenadas que le parezca más conveniente, pero debe ser cuidadoso de ser consistente con el mismo al programar el sistema de control. En este caso, el sistema de coordenadas utilizado tiene el eje z invertido, es decir, apuntando hacia abajo, como es convención en el mundo aeronáutico. Esto dependerá también de la IMU que hayamos elegido y de su orientación en la aeronave, hay que tener cuidado y realizar las pruebas respectivas (como ya hicimos en el numaral anterior) para asegurar que las lecturas obtenidas se ajusten a nuestro sistema de coordenadas.
Ahora bien, después de lo que hemos leído y aprendido sobre el control PID, ¿cómo implementarlo en nuestro Arduino para nuestra aeronave? Después de todo, nos quedan algunos interrogantes y consideraciones en nuestra situación particular, a saber:
Para esto es útil recurrir nuevamente a nuestro buen amigo, el diagrama de bloques (figura 7.3), que de una forma sencilla y gráfica nos presenta la conceptualización de nuestro sistema de control, resolviendo las inquietudes anteriores de esta forma:
Vamos entonces a analizar las partes del código que corresponden al sistema de control y simultáneamente aprovecharemos para esclarecer algunos conceptos sobre la estrategia seleccionada. Pero antes de ello debemos hacer un par de aclaraciones pertinentes repsecto a la estrategia de control utilizada.
Comenzamos entonces con nuestro código del sistema de control. En primera instancia declaramos las variables correspondientes al control, cuyo uso se verá más adelante. Por ahora debemos mencionar la variable SampleTime, la cual hemos inicializado en 4 (valor en milisegundos). Ya veremos su importancia y el porqué de ese valor.
En el "setup" comenzamos por establecer los valores límite para la señal de control, los cuales serán -400 y 400. ¿Por qué estos valores? Ya que esperamos que el valor central de cada ángulo esté en 1500 us, podemos movernos 500 us a cada lador alrededor de este valor. Por ello los valores de -400 y 400, donde dejamos un pequeño margen de 100 us a ambos lados. Inicializamos algunos valores en 0 para el cálculo de los términos derivativo e integral de cada controlador. Los valores de referencia iniciales serán 1500 para la referencia de los ángulos y 1000 para la señal de elevación. El 1500 se debe a que para los ángulos, dado que nos podemos desplazar hacia un lado o hacia el otro, dentro de un rango de 1000 a 2000 el valor central (sin desplazamiento) será en la mitad, es decir, 1500. También inicializamos en 0 las señales de salida de cada controlador.
A continuación, como se observa, usamos la función SetTunings para asignar los parámetros Kp (proporcional), Ki (integral) y Kd (derivativo) a cada controlador. Acá surge una pregunta clave para esta sección: ¿cómo se determinan estos valores? Existen varias alternativas, dependiendo del conocimiento técnico del constructor y el grado de refinación que se busque. En mi caso creé un modelo matemático del dron a partir de las ecuaciones físicas que gobiernan su comportamiento y lo implementé en Matlab, tomando también como parámetros las características físicas del aparato (peso, longitudes, etc) así como algunas características del Arduino (en especial la tasa de refresco que escogimos de 4 ms, o 250 Hz) para simular la aeronave y poder ajustar en el programa los valores del controlador PID hasta encontrar aquellos que brindaran un desempeño óptimo. Después de eso obtuve unos buenos valores de referencia con los que pude comenzar a hacer "ensayo y error" en la aeronave real, pero ya siendo los ajustes finales mínimos. Sin embargo, este procedimiento no hace parte del alcance de este tutorial, que debe mantenerse lo más sencillo posible. Así que por ahora recomiendo empezar con los valores que aparecen en este ejemplo, que fueron precisamente los obtenidos en mi experiencia, los cuales deben ser refinados posteriormente durante la puesta a punto de la aeronave de cada usuario.
En el "loop" comenzamos por establecer unos valores de ajuste. Como dijimos, vamos a utilizar los ángulos calculados de "roll" y de "pitch" para corregir el control por velocidad angular para los ejes x y y y lograr que el dron se "autonivele". Por ello asignamos los ángulos respectivos a las variables "roll_level_adjust" y "pitch_level_adjust" multiplicados por 15. Este valor es un número que indica la velocidad con la que queremos que se efectúe la corrección y el cual el lector puede modificar a su gusto, pero recomiendo empezar con este valor obtenido empíricamente.
Luego tenemos una sección dedicada al control de "roll". Establecemos un "set point" de cero. Recordemos que estamos controlando velocidad angular, por lo cual este "set point" nos dice que nuestra señal de velocidad angular deceada alrededor del eje x es de 0 °/s. Luego entramos a un condicional, donde básicamente establecemos que no haya ningún control si la señal de potencia es menor de 1200 us, esto con el fin de que la nave no comience a estabilizarse sin haberse elevado (y por lo tanto podemos arrancar de manera segura desde un terreno que no sea perfectamente plano, como el pasto). Posteriormente establecemos una pequeña "banda muerta" entre 1490 y 1510 us. Esto se debe a que no podemos asegurar que cuando la palanca del mando esté centrada, su valor enviado sea exactamente 1500 us, sino que posiblemente oscilará en un rango cercano a este valor. A continuación modificamos el "set point" restándole el valor de ajuste del ángulo de "roll" y dividimos este valor por tres para limitar un poco la velocidad máxima de giro. Finalmente, recordamos que al controlador le ingresan dos señales: la referencia y la medida del estado actual. La referencia es el "set point" que acabamos de modificar; la medida es la señal actual de velocidad angular en el eje x, llamda "gx_rate", así que asignamos este valor a la variable "Input_r".
La sección de "pitch" es exactamente igual a la de "roll", simplemente utilizando las variables correspondientes. Finalmente la sección de "yaw" es parecida, salvo que en este caso no tenemos necesidad de corregir el "set point" para auto-nivelación.
Acto seguido invocamos la función Compute(), que veremos más adelante, en la cual calcularemos la señal de salida de cada controlador PID. Luego limitamos el máximo valor de potencia a 1800 us, para dar un margen a las otras señales de control en el ventual caso en que operemos el dron a máxima potencia. Finalmente calculamos la salida a ser enviada a cada motor usando las cuatro señales mencionadas en la explicación de la figura 7.3: la señal de potencia y la suma o resta de la señal de salida de cada controlador. Como medida de seguridad limitamos la señal de cada motor a los valores ya conocidos, en el rango entre 1000 y 2000 y finalmente escribimos dichos valores al pin asignado a cada variador.
Al final del "loop" tenemos un par de líneas muy importantes. Habíamos definido una variable "loop_timer" la cual básicamente lleva la cuenta de lo que dura la ejecución de un "loop" en microsegundos. Acá restamos el valor actual en microsegundos (obtenido con la función micros()) de dicho valor, y nos quedamos dentro de un "while loop" hasta que esa diferencia sea mayor o igual a 4000, después de lo cual cargamos el nuevo valor de microsegundos a la variable "loop timer" En otras palabras, en estas líneas aseguramos que la duración del lazo sea de 4000 microsegundos, o 4 milisegundos. ¿Por qué? Para el cálculo de los parámetros integral y derivativo del control PID es necesario conocer exactamente el valor del tiempo transcurrido entre cada ejecución del lazo. Para ello podemos medir el tiempo transcurrido cada vez que se ejecuta el lazo, o bien asegurar siempre un tiempo de ejecución fijo. En este caso hemos escogido la segunda opción, así que debemos asegurar que el tiempo de ejecución del ciclo "loop" sea constante, y así los parámetros Ki y Kp serán también constantes. En mis propias pruebas, una vez integrados tanto las lecturas de la IMU, las comunicaciones con el receptor RF y el envío de señales a los motores, más los cálculos de los ángulos y las computaciones propias de cada controlador PID, teniendo en cuenta un parámetro importante como la velocidad de procesador del Arduino Uno (16 MHz), encontré que la ejecución de un "loop" tardaba algo más de 3 ms, midiendo siempre un tiempo variable pero cercano a los 3.4 ms. Por ello decidí redondear este valor a 4 ms, y garantizar este tiempo fijo para la ejecución del "loop". Este valor lo utilizaremos en las funciones auxiliares para calcular los parámetros del control PID.
En cuanto a las funciones adicionales definidas, explicaremos la más importante, Compute(). Las demás son bastante sencillas y se pueden comprender con la ayuda de los comentarios (setTunings() y setOutputLimits(). Para computar las señales de salida de cada controlador, primero debemos obtener la señal de error, que como sabemos de la teoría de control, es la diferencia entre el "set point" y la medición actual. Para calcular el término integral multiplicamos el error por el valor de la constnte integral (Ki) que cargamos en el "setup". Debemos limitar este valor a un mínimo o máximo establecido por los valores que también determinamos en el "setup" (-400 y 400). Para el término derivativo debemos calcular la diferencia entre la medición actual y la anterior, multiplicando también este valor por el kd establecido. Finalmente la señal de salida de cada controlador PID será compuesta por el término proporcional multiplicado por el error más los términos integral y derivativo ya computados. Para terminar guardamos la medición actual para ser usada en el próximo cálculo.
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Dron - Arduino
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Impementación de un UAV tipo cuadricóptero con motores Brushless (sin escobillas)
// El control se realiza usando la velocidad angular y haciendo correcciones según los ángulos calculados
// El control remoto está compuesto por transmisor y receptor
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Incluimos algunas librerías adicionales que vamos a utilizar
#include <Wire.h> // Librería para protocolo I2C
#include <Servo.h> // Librería para el manejo de los motores
// ALGUNAS DEFINICIONES
//Direccion I2C de la IMU
#define IMU_ADDRESS 0x68
// Variables control PID
unsigned long lastTime;
double Input_r, Output_r, Setpoint_r;
double Input_p, Output_p, Setpoint_p;
double Input_y, Output_y, Setpoint_y;
double Output_h;
double ITerm_r, lastInput_r;
double ITerm_p, lastInput_p;
double ITerm_y, lastInput_y;
double kp_r, ki_r, kd_r;
double kp_p, ki_p, kd_p;
double kp_y, ki_y, kd_y;
int SampleTime = 4; // 4 milisegundos
double outMin_dif, outMax_dif;
float roll_level_adjust, pitch_level_adjust;
// Offset para los acelerómetros (deben ser obtenidos en reposo en posición horizontal - usar el programa de la IMU)
float aoffsetX = -8, aoffsetY = -199, aoffsetZ = 3870;
// Variables IMU
int acc[3];
int gyro[3];
int temperature;
float rawX, rawY, rawZ;
float X, Y, Z;
float gyroX, gyroY, gyroZ;
float gx_rate, gy_rate, gz_rate;
float last_gx_rate, last_gy_rate, last_gz_rate;
float rollrad, pitchrad;
unsigned long mtime, looptime, loop_timer;
float angleAx, angleAy, angleAz;
float angleGx, angleGy, angleGz;
float goffsetX, goffsetY, goffsetZ;
// Variables del filtro
float angleFx, angleFy;
// Motores
Servo esc1, esc2, esc3, esc4;
char dato;
// Control remoto
volatile int rollref, pitchref, yawref, href;
byte last_channel_1, last_channel_2, last_channel_3, last_channel_4;
int receiver_input[4];
unsigned long timer_1, timer_2, timer_3, timer_4, current_time, last_int;
// Otras
int start;
int led_dis;
void setup()
{
// Deshabilitar watchdog antes del setup
wdt_disable();
// Inicio del puerto serial a 115200 baudios y mensaje de bienvenida
Serial.begin(115200);
Serial.println("Bienvenido, sistema iniciando...");
// Inicio de comunicación I2C
Wire.begin();
// Inicializamos la IMU
init_IMU();
// Asignamos cada motor a su pin correspondiente
esc1.attach(8,1000,2000);
esc2.attach(9,1000,2000);
esc3.attach(10,1000,2000);
esc4.attach(11,1000,2000);
esc1.write(0); // ¡IMPORTANTE! Hay que armar cada motor
esc2.write(0);
esc3.write(0);
esc4.write(0);
// Los pines que se utilicen como salida deben declararse explícitamente - en este caso, el LED
pinMode(LED_PIN, OUTPUT);
// Un pequeño "delay" e iniciamos la calibración de la IMU
delay(1000);
Serial.println("Calibrando...");
Serial.println("Mantener la IMU quieta y en posición horizontal");
calibrate();
delay(500);
Serial.println("Calibración finalizada.");
// Inicialización de los parámetros de control
double *limsdif = SetOutputLimits(-400, 400);
outMin_dif = limsdif[0];
outMax_dif = limsdif[1];
ITerm_r = 0, lastInput_r = 0;
ITerm_p = 0, lastInput_p = 0;
ITerm_y = 0, lastInput_y = 0;
// Los controles se asumen centrados al comienzo
rollref = 1500;
pitchref = 1500;
yawref = 1500;
href = 0; // Lo iniciamos en cero como valor seguro.
Output_r = 0, Output_p = 0; Output_y = 0;
// Roll Control
double *cons_r = SetTunings(0.8, 0.1, 0.02);
kp_r = cons_r[0];
ki_r = cons_r[1];
kd_r = cons_r[2];
// Pitch Control
// Igualamos a los parámetros de "roll" ya que el dron es prácticamente simétrico en los ejes x y y
double *cons_p = cons_r;
kp_p = cons_p[0];
ki_p = cons_p[1];
kd_p = cons_p[2];
// Yaw Control
double *cons_y = SetTunings(2, 0.01, 0.01);
kp_y = cons_y[0];
ki_y = cons_y[1];
kd_y = cons_y[2];
// Configuramos las interrupciones para recibir los comandos remotos
PCICR |= (1 << PCIE2); // PCIE2 activado para habilitar PCMSK2 scan.
PCMSK2 |= (1 << PCINT23); // PCINT23 (D7) activado para disparar una interrupción si hay cambio de estado.
PCMSK2 |= (1 << PCINT22); // PCINT22 (D6) activado para disparar una interrupción si hay cambio de estado.
PCMSK2 |= (1 << PCINT21); // PCINT21 (D5) activado para disparar una interrupción si hay cambio de estado.
PCMSK2 |= (1 << PCINT20); // PCINT20 (D4) activado para disparar una interrupción si hay cambio de estado.
// Esperamos hasta que el receptor esté activo y la señal de potencia esté al mínimo.
while(href < 900 || href > 1150 || yawref < 1400){
href = receiver_input[0];
yawref = receiver_input[3];
start ++; // Mientras esperamos incrementamos la variable "start" en cada ciclo.
// Enviamos repetidamente una señal mínima a los motores para mantenerlos armados en posición segura
esc1.writeMicroseconds(1000);
esc2.writeMicroseconds(1000);
esc3.writeMicroseconds(1000);
esc4.writeMicroseconds(1000);
delay(3);
if (start == 125) {
digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // ...cambiamos el estado del LED
start = 0; // Reiniciamos "start"
}
}
start = 0; // Reiniciamos "start"
led_dis = 1; // LED arranca en estado encendido
Serial.println("Listo para volar");
mtime = millis(); // Inicializamos la variable mtime con el valor actual en milisegundos (para uso del giroscopio)
loop_timer = micros(); // Inicializamos la variable mtime con el valor actual en milisegundos (para uso de las interrupciones)
}
void loop()
{
wdt_reset(); // Reset watchdog cada loop
// Leemos las entradas del transmisor remoto
href = receiver_input[0];
rollref = receiver_input[1];
pitchref = receiver_input[2];
yawref = receiver_input[3];
// Leemos la IMU
read_IMU();
// Obtenemos ángulos de roll and pitch de lecturas del acelerómetro
rawX = acc[0] - aoffsetX;
rawY = acc[1] - aoffsetY;
rawZ = acc[2] - (4095 - aoffsetZ); // Debemos considerar la gravedad; en este caso 1g es igual a 4095
// 1/4096 = 0.0002 (4096 es la sensibilidad según datasheet para el rango que escogimos, usamos el inverso para multiplicar en vez de dividir)
X = rawX*0.0002;
Y = rawY*0.0002;
Z = rawZ*0.0002;
rollrad = atan(Y/sqrt(X*X+Z*Z)); // Ángulo en radianes
pitchrad = atan(X/sqrt(Y*Y+Z*Z)); // Ángulo en radianes
// 180/pi = 57.2958 (factor para convertir a grados)
angleAx = rollrad*57.2958; // Ángulo de roll en grados
angleAy = pitchrad*57.2958; // Ángulo de pitch en grados
// Código para giroscopio (roll, pitch, yaw)
looptime = (millis() - mtime); // Calculamos el valor transcurrido en ms entre el ciclo pasado y este
mtime = millis(); // Guardamos el valor actual para usarlo en el próximo ciclo
gyroX = gyro[0] - goffsetX;
gyroY = gyro[1] - goffsetY;
gyroZ = gyro[2] - goffsetZ;
// 1/65.5 = 0.0153 (65.5 es la sensibilidad según datasheet para el rango que escogimos, usamos el inverso para multiplicar en vez de dividir)
gx_rate = gyroX*0.0153;
gy_rate = gyroY*0.0153;
gz_rate = gyroZ*0.0153;
// Filtrado inicial de las mediciones de velocidad angular usando una proporción de la medida anterior y la actual
gx_rate = (0.7*last_gx_rate) + (0.3*gx_rate);
gy_rate = (0.7*last_gy_rate) + (0.3*gy_rate);
gz_rate = (0.7*last_gz_rate) + (0.3*gz_rate);
// Integramos para obtener los ángulos
angleGx = angleGx + (gx_rate * looptime)*0.001; // Convertir a segundos (1/1000)
angleGy = angleGy - (gy_rate * looptime)*0.001; // Medición de pitch rate está invertida por sistema de coordenadas elegido
angleGz = angleGz - (gz_rate * looptime)*0.001; // Medición de yaw rate está invertida por sistema de coordenadas elegido
// Limitamos los ángulos al rango entre -180 y 180 grados
if (angleGx < -180) angleGx += 360;
if (angleGx > 180) angleGx -= 360;
if (angleGy < -180) angleGy += 360;
if (angleGy > 180) angleGy -= 360;
if (angleGz < 0) angleGz += 360;
if (angleGz > 360) angleGz -= 360;
// Guardar valores actuales para el próximo ciclo
last_gx_rate = gx_rate;
last_gy_rate = gy_rate;
last_gz_rate = gz_rate;
// Corrección de "yaw" para los ángulos obtenidos del giroscopio
// pi/180 = 0.0174 (para convertir a radianes, luego *0.001 para convertir a segundos
angleFx -= angleFy * sin((gz_rate * looptime)*0.0000174); // Si hubo "yaw" tranferir ángulo de pitch a roll
angleFy += angleFx * sin((gz_rate * looptime)*0.0000174); // Si hubo "yaw" tranferir ángulo de roll a pitch
// Aplicar el Filtro Complementario a los ángulos originales
angleFx = 0.995*(angleFx + (gx_rate * looptime*0.001)) + 0.005*angleAx;
angleFy = 0.995*(angleFy - (gy_rate * looptime*0.001)) + 0.005*angleAy;
// Limitamos los ángulos al rango entre -180 y 180 grados
if (angleFx < -180) angleFx += 360;
if (angleFx > 180) angleFx -= 360;
if (angleFy < -180) angleFy += 360;
if (angleFy > 180) angleFy -= 360;
// Corrección de velocidad angular para autonivelación
roll_level_adjust = angleFx * 15; // Calcular corrección de roll
pitch_level_adjust = angleFy * 15; // Calcular corrección de pitch
// ------------------------------ CONTROL PID --------------------------------------
// Roll Control
// El set point en grados por segundo es determinado de la señal recibida del mando remoto.
// Dividimos por 3 para limitar el máximo "roll rate" a unos 163 grados por segundo ( (500-10)/3 = 163d/s ).
Setpoint_r = 0;
if (href > 1200){ // No hay control si la palanca de potencia está abajo
// Establecemos una banda muerta de 20us para un mejor rendimiento
if (rollref > 1510) Setpoint_r = rollref - 1510;
else if (rollref < 1490) Setpoint_r = rollref - 1490;
Setpoint_r -= roll_level_adjust; // Restar la corrección calculada preciamente para autonivelar
Setpoint_r *= 0.3333; // Dividir el setpoint por 3 para obtener ángulos en grados.
}
Input_r = gx_rate; // Como la variable a controlar es la velocidad angular, es este el valor que se pasa como entrada al control PID
// Pitch Control
// El set point en grados por segundo es determinado de la señal recibida del mando remoto.
// Dividimos por 3 para limitar el máximo "pitch rate" a unos 163 grados por segundo ( (500-10)/3 = 163d/s ).
Setpoint_p = 0;
if (href > 1200){ // No hay control si la palanca de potencia está abajo
// Establecemos una banda muerta de 20us para un mejor rendimiento
if (pitchref > 1510) Setpoint_p = pitchref - 1510;
else if (pitchref < 1490) Setpoint_p = pitchref - 1490;
Setpoint_p -= pitch_level_adjust; // Restar la corrección calculada preciamente para autonivelar
Setpoint_p *= 0.3333; // Dividir el setpoint por 3 para obtener ángulos en grados.
}
Input_p = -gy_rate; // Como la variable a controlar es la velocidad angular, es este el valor que se pasa como entrada al control PID
// Yaw Control
// El set point en grados por segundo es determinado de la señal recibida del mando remoto.
// Dividimos por 3 para limitar el máximo "yaw rate" a unos 163 grados por segundo ( (500-10)/3 = 163d/s ).
Setpoint_y = 0;
// Establecemos una banda muerta de 20us para un mejor rendimiento
if (href > 1200){ // No hay control si la palanca de potencia está abajo
if (yawref > 1510) Setpoint_y = (yawref - 1510)*0.3333;
else if (yawref < 1490) Setpoint_y = (yawref - 1490)*0.3333;
}
Input_y = gz_rate; // Como la variable a controlar es la velocidad angular, es este el valor que se pasa como entrada al control PID
// Todos los cálculos de control PID
Compute();
// Para señal de potencia se envía el valor recibido directamente del receptor
Output_h = href;
if (Output_h > 1800) Output_h = 1800; // Dejamos un margen para poder realizar control a máxima potencia
// Calculamos la salida a cada motor combinando adecuadamente la señal de salida de cada controlador
int Output1 = Output_h + Output_r + Output_p - Output_y;
int Output2 = Output_h - Output_r + Output_p + Output_y;
int Output3 = Output_h - Output_r - Output_p - Output_y;
int Output4 = Output_h + Output_r - Output_p + Output_y;
// Limitamos los valores de ancho de pulso entre 1000 y 2000 us
if (Output1 > 2000) Output1 = 2000;
else if (Output1 < 1000) Output1 = 1000;
if (Output2 > 2000) Output2 = 2000;
else if (Output2 < 1000) Output2 = 1000;
if (Output3 > 2000) Output3 = 2000;
else if (Output3 < 1000) Output3 = 1000;
if (Output4 > 2000) Output4 = 2000;
else if (Output4 < 1000) Output4 = 1000;
// Si la salida es más baja de 1100 microsegundos, mantenemos los motores corriendo a baja potencia
if (Output1 < 1100) Output1 = 1100;
if (Output2 < 1100) Output2 = 1100;
if (Output3 < 1100) Output3 = 1100;
if (Output4 < 1100) Output4 = 1100;
// Enviamos la respectiva señal a cada motor
esc1.writeMicroseconds(Output1);
esc2.writeMicroseconds(Output2);
esc3.writeMicroseconds(Output3);
esc4.writeMicroseconds(Output4);
// El programa debe correr a 250Hz. Eso significa que debemos asegurar un ciclo cada 4ms.
while (micros() - loop_timer < 4000); // Esperamos a que transcurran 4000us
loop_timer = micros(); // Actualizamos el valor del timer para el siguiente ciclo
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// RUTINA DE INTERRUPCIÓN
// Esta rutina es invocada cada vez que alguno de los pines 7, 6, 5 o 4 cambia de estado, para leer las señales del receptor.
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ISR(PCINT2_vect){
current_time = micros();
//Channel 1=========================================
if(PIND & B10000000){ // ¿El pin 7 está ALTO?
if(last_channel_1 == 0){ // Si cambió de 0 a 1...
last_channel_1 = 1; // Guardar estado actual.
timer_1 = current_time; // Guardar tiempo actual en timer_1.
}
}
else if(last_channel_1 == 1){ // Si el pin 7 está en BAJO y pasó de 1 a 0.
last_channel_1 = 0; // Guardar estado actual.
receiver_input[0] = current_time - timer_1; // El valor de este canal es current_time - timer_1.
}
//Channel 2=========================================
if(PIND & B01000000 ){ // ¿El pin 6 está ALTO?
if(last_channel_2 == 0){ // Si cambió de 0 a 1...
last_channel_2 = 1; // Guardar estado actual.
timer_2 = current_time; // Guardar tiempo actual en timer_2.
}
}
else if(last_channel_2 == 1){ // Si el pin 6 está en BAJO y pasó de 1 a 0.
last_channel_2 = 0; // Guardar estado actual.
receiver_input[1] = current_time - timer_2; // El valor de este canal es current_time - timer_2.
}
//Channel 3=========================================
if(PIND & B00100000 ){ // ¿El pin 5 está ALTO?
if(last_channel_3 == 0){ // Si cambió de 0 a 1...
last_channel_3 = 1; // Guardar estado actual.
timer_3 = current_time; // Guardar tiempo actual en timer_3.
}
}
else if(last_channel_3 == 1){ // Si el pin 5 está en BAJO y pasó de 1 a 0.
last_channel_3 = 0; // Guardar estado actual.
receiver_input[2] = current_time - timer_3; // El valor de este canal es current_time - timer_3.
}
//Channel 4=========================================
if(PIND & B00010000 ){ // ¿El pin 4 está ALTO?
if(last_channel_4 == 0){ // Si cambió de 0 a 1...
last_channel_4 = 1; // Guardar estado actual.
timer_4 = current_time; // Guardar tiempo actual en timer_4.
}
}
else if(last_channel_4 == 1){ // Si el pin 4 está en BAJO y pasó de 1 a 0.
last_channel_4 = 0; // Guardar estado actual.
receiver_input[3] = current_time - timer_4; // El valor de este canal es current_time - timer_4.
}
last_int = current_time; // Guardamos el tiempo de la última interrupción de cualquier pin
}
// ------------Funciones complementarias---------------
// Inicializar la IMU
void init_IMU() {
Wire.beginTransmission(IMU_ADDRESS); // Comenzar comunicación con la dirección de la IMU.
Wire.write(0x6B); // Escribir al registro PWR_MGMT_1 (6B hex)
Wire.write(0x00); // Escribir 00000000 para activar el giroscopio
Wire.endTransmission(); // Finalizar la transmisión
Wire.beginTransmission(IMU_ADDRESS); // Comenzar comunicación con la dirección de la IMU.
Wire.write(0x1B); // Escribir al registro GYRO_CONFIG (1B hex)
Wire.write(0x08); // Escribir 00001000 (Rango 500dps)
Wire.endTransmission(); // Finalizar la transmisión
Wire.beginTransmission(IMU_ADDRESS); // Comenzar comunicación con la dirección de la IMU.
Wire.write(0x1C); // Escribir al registro ACCEL_CONFIG (1A hex)
Wire.write(0x10); // Escribir 00010000 (Rango +/- 8g)
Wire.endTransmission(); // Finalizar la transmisión
// Pequeña revisión para verificar los valores enviados
Wire.beginTransmission(IMU_ADDRESS); // Comenzar comunicación con la dirección de la IMU.
Wire.write(0x1B); // Leer registro 0x1B
Wire.endTransmission(); // Finalizar la transmisión
Wire.requestFrom(IMU_ADDRESS, 1); // Solicitar 1 byte
while(Wire.available() < 1); // Esperar la recepción de la información
if(Wire.read() != 0x08){ // Verificar que el valor es 0x08
while(1) delay(10); // Quedarse en este "loop"
}
Wire.beginTransmission(IMU_ADDRESS); // Comenzar comunicación con la dirección de la IMU.
Wire.write(0x1A); // Escribir al registro CONFIG (1A hex)
Wire.write(0x03); // Escribir 00000011 (Low Pass Filter to ~43Hz)
Wire.endTransmission(); // Finalizar la transmisión
}
// Leer la IMU
void read_IMU(){
Wire.beginTransmission(IMU_ADDRESS); // Comenzar comunicación con la dirección de la IMU.
Wire.write(0x3B); // Leer desde registro 3B
Wire.endTransmission(); // Finalizar la transmisión
Wire.requestFrom(IMU_ADDRESS,14); // Solicitar 14 bytes
while(Wire.available() < 14); // Esperar recepción de la información
acc[0] = Wire.read()<<8|Wire.read(); // Concatenar low y high byte de acelerómetro en eje x.
acc[1] = Wire.read()<<8|Wire.read(); // Concatenar low y high byte de acelerómetro en eje y.
acc[2] = Wire.read()<<8|Wire.read(); // Concatenar low y high byte de acelerómetro en eje z.
temperature = Wire.read()<<8|Wire.read(); // Concatenar low y high byte en la variable temperature.
gyro[0] = Wire.read()<<8|Wire.read(); // Concatenar low y high byte de giroscopio en eje x.
gyro[1] = Wire.read()<<8|Wire.read(); // Concatenar low y high byte de giroscopio en eje y.
gyro[2] = Wire.read()<<8|Wire.read(); // Concatenar low y high byte de giroscopio en eje z.
}
// Rutina de calibración
void calibrate() {
// Leemos cien datos de la IMU y obtenemos los respectivos promedios
for (int i = 0; i < 100; i++) {
read_IMU();
if (i == 0) {
goffsetX = gyro[0];
goffsetY = gyro[1];
goffsetZ = gyro[2];
}
if (i > 0) {
goffsetX = (gyro[0] + goffsetX);
goffsetY = (gyro[1] + goffsetY);
goffsetZ = (gyro[2] + goffsetZ);
}
}
goffsetX = goffsetX*0.01;
goffsetY = goffsetY*0.01;
goffsetZ = goffsetZ*0.01;
// Los valores obtenidos son el "offset" inicial de los giroscopios
Serial.print("Los offset del GYRO son: ");
Serial.print(goffsetX);
Serial.print(", ");
Serial.print(goffsetY);
Serial.print(", ");
Serial.println(goffsetZ);
// El "offset" inicial de los acelerómetros debe ser introducido arriba manualmente, dependiendo de su calibración EN REPOSO
Serial.print("Los offset del ACC son: ");
Serial.print(aoffsetX);
Serial.print(", ");
Serial.print(aoffsetY);
Serial.print(", ");
Serial.println(aoffsetZ);
// Volvemos a leer la IMU para obtener datos promedio del acelerómetro
float acc_x = 0, acc_y = 0, acc_z = 0;
for (int i = 0; i < 10; i++) {
read_IMU();
acc_x += acc[0];
acc_y += acc[1];
acc_z += acc[2];
}
// Promedio de 10 lecturas
acc_x *= 0.1;
acc_y *= 0.1;
acc_z *= 0.1;
rawX = acc_x - aoffsetX;
rawY = acc_y - aoffsetY;
rawZ = acc_z - (4095 - aoffsetZ); // Debemos considerar la gravedad; en este caso 1g es igual a 4095
// 1/4096 = 0.0002 (4096 es la sensibilidad según datasheet para el rango que escogimos, usamos el inverso para multiplicar en vez de dividir)
X = rawX*0.0002;
Y = rawY*0.0002;
Z = rawZ*0.0002;
rollrad = atan(Y/sqrt(X*X+Z*Z)); // Ángulo en radianes
pitchrad = atan(X/sqrt(Y*Y+Z*Z)); // Ángulo en radianes
// 180/pi = 57.2958 (factor para convertir a grados)
angleAx = rollrad*57.2958; // Ángulo de roll en grados
angleAy = pitchrad*57.2958; // Ángulo de pitch en grados
// Inicializamos los filtros con estos ángulos
angleGx = angleAx;
angleGy = angleAy;
angleFx = angleAx;
angleFy = angleAy;
}
//--------Funciones del control PID------
void Compute() {
// Calculamos todos los errores
double error_r = Setpoint_r - Input_r;
double error_p = Setpoint_p - Input_p;
double error_y = Setpoint_y - Input_y;
ITerm_r += (ki_r * error_r);
if (ITerm_r > outMax_dif) ITerm_r = outMax_dif;
else if (ITerm_r < outMin_dif) ITerm_r = outMin_dif;
ITerm_p += (ki_p * error_p);
if (ITerm_p > outMax_dif) ITerm_p = outMax_dif;
else if (ITerm_p < outMin_dif) ITerm_p = outMin_dif;
ITerm_y += (ki_y * error_y);
if (ITerm_y > outMax_dif) ITerm_y = outMax_dif;
else if (ITerm_y < outMin_dif) ITerm_y = outMin_dif;
double dInput_r = (Input_r - lastInput_r);
double dInput_p = (Input_p - lastInput_p);
double dInput_y = (Input_y - lastInput_y);
// Calculamos la función de salida del PID.
Output_r = (kp_r * error_r) + ITerm_r - (kd_r * dInput_r);
if (Output_r > outMax_dif) Output_r = outMax_dif;
else if (Output_r < outMin_dif) Output_r = outMin_dif;
Output_p = (kp_p * error_p) + ITerm_p - (kd_p * dInput_p);
if (Output_p > outMax_dif) Output_p = outMax_dif;
else if (Output_p < outMin_dif) Output_p = outMin_dif;
Output_y = (kp_y * error_y) + ITerm_y - (kd_y * dInput_y);
if (Output_y > outMax_dif) Output_y = outMax_dif;
else if (Output_y < outMin_dif) Output_y = outMin_dif;
// Guardamos el valor de algunas variables para el próximo recálculo.
lastInput_r = Input_r;
lastInput_p = Input_p;
lastInput_y = Input_y;
}
double *SetTunings (double Kp, double Ki, double Kd) {
static double tunes[3];
int i;
if (Kp<0 || Ki<0|| Kd<0) {
tunes[0] = 0;
tunes[1] = 0;
tunes[2] = 0;
return tunes;
}
double SampleTimeInSec = ((double)SampleTime)/1000;
tunes[0] = Kp;
tunes[1] = Ki * SampleTimeInSec;
tunes[2] = Kd / SampleTimeInSec;
return tunes;
}
double *SetOutputLimits(double Min, double Max) {
static double lims[2];
if (Min > Max) {
lims[0] = Max;
lims[1] = Min;
} else {
lims[0] = Min;
lims[1] = Max;
}
return lims;
}
Ya tenemos prácticamente listo nuestro programa en Arduino. Ya sabemos integrar las señales de la IMU, del módulo de comunicaciones, de los ESC/motores y ya hemos implementado nuestro sistema de control. Como último paso es conveniente refinar un poco el código, agregando algunas mejoras simples pero útiles, como las siguientes:
No explicaremos en detalle el código de estas mejoras, el cual es bastante simple y puede comprenderse correctamente a través de los comentarios insertados en el mismo.
Ahora sí, nuestro programa está listo. En este enlace se puede descargar el código completo, que incluye de manera integral todo lo que ya hemos visto. Recomiendo leerlo, en especial los comentarios, y familiarizarse con él y sus partes, ya que en las pruebas puede ser necesario modificar algún parámetro y convienen saber identificar su ubicación rápidadmente. Una vez listos, podemos proceder a descargarlo en nuestra placa Arduino.
Asegurando que tenemos la batería en un nivel aceptable de operación (conviene tener un multímetro para este propósito), podemos ensamblar todos los componentes faltantes sobre el marco del dron, exceptuando en lo posible los rotores. Haremos una primera prueba de encendido y apagado sin ellos para asegurar que el dron opere en las condiciones esperadas. El transmisor debe encenderse primero en lo posible, y apagarse de último también. Una vez estemos listos, podemos energizar el dron conectando la batería a su respectivo conector. Deberíamos escuchar los pitidos de los motores al armarse y luego deberíamos ver el LED encendido. Significa que estamos listos y podemos proceder a manipular el dorn con el mando remoto, recordando el procedimiento de arranque. Los motores deberían rotar a una velocidad mínima que podemos ir aumentando con la palanca de potencia. Podemos utilizar las demás palancas y ver como la potencia de los motores se aplica en forma diferencial. Finalmente podemos apagar los motores con el procedimiento que ya conocemos y desconectar la batería.
Si todo marchó bien, ahora sí es un buen momento para acoplar los rotores a los motores, teniendo en cuenta el sentido de dirección. Es recomendable hacer este paso al final, ya que hacer las pruebas previas de los motores con las hélices ya puestas es peligroso para la integridad de la aeronave y de las personas alrededor en caso de que algo no haya quedado bien (error en el programa, problemas de conexión con algún componente, etc.). Ahora sí estamos listos para volar la aeronave, por lo cual debemos buscar una ubicación segura para hacerlo.
Para volar la aeronave repetiremos los pasas anteriores. Esta vez al subir la potencia la nave debería elevarse manteniendo una actitud nivelada, después de lo cual podemos probar los demás controles y ver la reacción del dron ante los mismos. Si estamos satisfechos, ¡hemos terminado! Tenemos nuestro propio dron operativo, hecho por nosotros mismos. Si el control vibra mucho o es definitivamente inestable, debemos revisar el programa de control y ajustar los parámetros de los controladores PID hasta que el control sea eficiente. Los parámetros del controlador de "roll" y de "pitch" deberían ser exactamente iguales, pues la aeronave es simétrica respecto a los ejes x y y (de hecho al montar los componentes esta simetría se altera ligeramente, pero no de una manera significativa, por lo que podemos seguir asumiendo dicha simetría tranquilamente).
Una vez que todo está listo, ¡a volar se dijo!
Mira acá el video correspondiente a los pasos 7, 8 y 9 del tutorial.
Algún problema pudo haber surgido durante la construcción que solo vendrá a manifestarse, como bien dicta la ley de Murphy, al final del proceso. Para ello es conveniente revisar meticulosamente todo el proceso e identificar el problema:
Mira acá el video correspondiente a este paso del tutorial.