CONSTRUCCIÓN

Continuamos con la guía, pasos 6 a 10.



Paso 6: Conexión de componentes

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:

  • La IMU, como se mencionó, se comunica por el protocolo I2C, lo que supone usar los pines SCL y SDA del Arduino. Adicionalmente requiere alimentación (es posible usar la salida de 3.3 V o la de 5 V) y conexión a tierra.
  • El receptor de comunicaciones cuenta con varios pines, de los cuales usaremos cuatro señales (throttle, ailerons, elevator and rudder, usando el nombre que suelen tener estas marquillas en los receptores) conectados a los pines 7, 6, 5 y 4 del Arduino respectivamente. Hay una hilera completa de pines para alimentación (debe ser a 5V) y otra para tierra, de las cuales solo hace falta usar una de cada una.
  • Para los motores/variadores comenzaremos a contar desde el de adelante a la izquierda y nos desplazaremos así en sentido horario hasta el 4. En ese orden, los conectaremos a los pines 8, 9, 10 y 11 del Arduino respectivamente. Las tierras ya están interconectadas entre sí en el momento en el que soldamos los variadores a la tarjeta de distribución, así que solo hace falta conectar una de ellas al Arduino. La señal de BEC de los variadores, si la tenemos, no será requerida en este tutorial.
  • El conector de la batería ya debería estar soldado a la tarjeta de distribución, por lo cual los ESC ya están siendo alimentados. Pero, importante, falta alimentar el Arduino. Según especificaciones obtenidas en su página, este puede recibir hasta unos 12 voltios (rango recomendado), por lo cual es posible alimentarlo directamente desde la batería. Para esto tenemos dos opciones: podemos utilizar un conector de 2.1 mm compatible con el jack de alimentación que tiene el Arduino y a través de un para de cables soldar a los mismos pines donde tenemos el conector de la batería. O bien es posible conectar un cable desde el pin positivo de la batería al pin llamado Vin de nuestro Arduino. En este segundo caso debemos utilizar un diodo para aislar la batería de otras posibles fuentes de alimentación (por ejemplo, el puerto USB de nuestro computador cuando hagamos pruebas). El diodo no es necesario en el primer caso puesto que ya está incluido entre el jack de alimentación y el pin Vin en la tarjeta Arduino.
  • Sea cual sea la opción elegida en el paso anterior, debemos conectar el pin Vin a un par de resistencias en serie. Esto con el fin de medir el nivel de voltaje de la batería. Las entradas análogas reciben un máximo de 5 V; por ende debemos convertir el valor de la batería (rango recomendado de operación entre 10 y 12.4 V) en un valor apropiado para el puerto del Arduino. Esto se hace con un divisor de voltaje (clic acá para leer lo que es esto), conformado por un par de resistencias de valores apropiados. No hay una única solución de valores, el lector puede utilizar las que le convengan conforme a la ecuación presentada en el enlace. En este caso sugiero unas resistencias de 4.7 y 10 kilo-ohmios; con estos valores el pin A5 del Arduino recibirá un máximo de 3.96 V, perfectamente aceptable.
  • Finalmente, un LED de indicación, aunque no necesario, es sugerido para tener una referencia visual del estado del dron. En este tutorial lo conectaremos al pin 2 del Arduino, junto a su resistencia de limitación de corriente, para la cual una de 470 ohmios o similar podrá servir.
Conexiones
Fig. 6 - Esquema de conexiones completas de dron (clic en la imagen para agrandar)

Mira acá el video correspondiente a este paso del tutorial.



Paso 7: Diseño y programación del control

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.

Control PID
Fig. 7.1 - Esquema de control del dron

Coordenadas
Fig. 7.2 - Definición de coordenadas

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:

  • El sistema de coordenadas que usaremos (ejes x-y-z). Con esto definiremos qué dirección será "adelante", "atrás" y demás para nuestro aparato, lo cuál es esencial para relacionar con nuestro sistema de comunicaciones y su integración con el sistema de control, asegurando que la señal de salida producida sea la esperada.
  • El sentido de rotación de las aspas. Como vimos en los fundamentos, los rotores contiguos deben girar en sentidos opuestos, donde finalmente tendremos dos girando en sentido CW (dirección horaria) y dos en sentido CCW (dirección antihoraria).

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:

  • Hemos aprendido sobre el control PID, pero lo que hemos visto es aplicable para sistemas SISO (Single Input - Single Output) o de una entrada y una salida. Pero en nuestro caso tenemos cuatro entradas (las señales de referencia de potencia, alabeo, cabeceo y guiñada, enviadas a través del transmisor remoto) y cuatro salidas, que son las salidas a cada motor. ¿Cómo hacemos entonces?
  • Tenemos cuatro entradas y cuatro salidas, pero en cuanto a las medidas obtenidas por nuestro sensor (la IMU) solo tenemos tres de ellas (ángulo de alabeo, de cabeceo y de guiñada). ¿Cómo integramos entonces estas tres medidas al sistema de control?
  • La salida del sistema de control, que como sabemos debe ser la entrada a los actuadores (los motores), ¿sale directamente del controlador o requiere algún tipo de ajuste antes de poder ser enviada efectivamente a cada motor?

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:

  • Al tener varias cantidades de entradas, salidas y mediciones, esto parece sugerirnos que realmente no necesitamos uno sino varios controladores. Esto es correcto. Como se observa en la figura, realmente debemos implementar TRES controladores en uno, uno para cada ángulo de actitud del dron (en adelante los llamaremos, a mi pesar, por su nombre en inglés - roll, pitch y yaw -, pues es una práctica ampliamente difundida en la industria). En efecto, tendremos tres controladores SISO.
  • En el caso de la potencia, que se traduce en la elevación de la aeronave, realmente no estamos efectuando ningún control y por eso vemos que esta señal "pasa derecho", sin requerir ningún controlador. En otras palabras, el valor de potencia que enviamos con nuestro transmisor es prácticamente el mismo que es usado para generar la señal de los motores.
  • Ahora bien, la señal que produce cada controlador, junto con la señal extra de potencia-elevación, deben ser combinadas entre sí de forma correcta para ser enviadas a cada variador-motor. Acá es donde vemos más claramente la importancia de las definiciones establecidas en la figura 7.2 (y donde debemos tener cuidado de ser coherentes en caso de haber escogido un sistema diferente). Debemos distinguir entre dos formas de aplicar estas señales a los motores: de manera integral (igual para todos los motores) o de manera diferencial (positiva para algunos motores y negativa para otros). Veamos cómo aplicamos cada una de las señales que tenemos:
    • Comencemos por la más fácil. La señal de elevación-potencia es la única que se aplica de forma integral a los cuatro motores. Por eso vemos que en cada bloque adonde llega la línea amarilla, su aplicación es positiva (representada por el signo '+'). Esto es lógico, puesto que la potencia suministrada al dron para elevarse debe ser suministrada por igual a todos los motores.
    • La señal de "roll" (rojo escarlata) es aplicada de forma diferencial, siendo positiva para los motores 1 y 4 y negativa para los motores 2 y 3. La razón la observamos en la figura 7.2. El "roll" es la rotación efectuada alrededor del eje x. Utilizando la convención de la mano derecha (según la cual debemos usar nuestra mano derecha, apuntando el pulgar en dirección del eje, donde la rotación del resto de dedos nos dice en qué sentido la rotación es positiva), tenemos que cuando los rotores 1 y 4 tengan más potencia, la rotación alrededor de este eje será positiva; cuando los rotores 2 y 3 reciban más potencia, la rotación será negativa. Esto explica la distribución de los signos '+' y '-' para esta señal.
    • Utilizamos el mismo razonamiento para la señal de "pitch" (azul), aplicada de forma diferencial y siendo positiva para los motores 3 y 4 y negativa para los motores 1 y 2, ya que el "pitch" es la rotación efectuada alrededor del eje y.
    • La señal de "yaw" (verde) también es aplicada de forma diferencial, siendo positiva para los motores 2 y 4 y negativa para los motores 1 y 3, ya que el "yaw" es la rotación efectuada alrededor del eje z. Teniendo en cuenta que la rotación de una hélice en un sentido provoca una rotación en el sentido opuesto para la aeronave podemos comprender, por ejemplo, para el rotor 2 que gira en sentido horario que el resultado final sobre la aeronave será un movimiento en sentido antihorario, que es el sentido positivo de giro alrededor del eje z. Con el mismo razonamiento para los demás rotores podemos explicar la distribución de los signos '+' y '-' para esta señal.
Diagrama de bloques sistema de control
Fig. 7.3 - Diagrama de bloques detallado del sistema de control

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.

  1. Recordemos las mediciones que se obtienen de la IMU. De los giroscopios obtenemos la velocidad angular alrededor de los ejes x, y y z. De los acelerómetros obtenemos, desde luego, la aceleración en cada dirección. Realmente no obtenemos de forma directa la medida de los ángulos de "roll", "pitch" y "yaw", sino que estos deben ser calculados matemáticamente a partir de la información anterior, como vimos en la sección 5.1. En ese caso, una estrategia de control que resulta conveniente es no controlar directamente el valor deseado de esos ángulos, sino la velocidad angular, para utilizar directamente la medición obtenida de los giroscopios y aprovechar sus características ventajosas, como el bajo nivel de ruido y la alta precisión. En resumen, al tirar las palancas de nuestro mando remoto no buscaremos inclinar el dron en un ángulo específico hacia la dirección adonde tiremos la palanca, sino que estableceremos más bien la rapidez con la cual el dron rotará en esa dirección. Ya veremos la implicación de esta consideración aparentemente sutil en el código.
  2. Sin embargo, cuando no estemos manipulando ninguna palanca aparte de la potencia, ¿en qué dirección cree el lector que se inclinará el dron? ¿Se mantendrá acaso perfectamente nivelado? La respuesta es que no, el dron mantendrá una inclinación que dependerá en gran medida de la última dirección en la que se haya tirado alguna palanca de dirección o incluso de la posición inicial del dron en el suelo. Esto precisamente porque estamos controlando velocidad angular y no posición, con lo cual la aeronave podrá, por ejemplo, mantener un ángulo de 10° hacia la derecha. Mientras no esté rotando, los giroscopios medirán "cero" y el sistema se estabilizará en esa posición. Esto no es del todo malo, de hecho en los drones de competencias el sistema de control se comporta de esta manera debido a la alta velocidad de respuesta. Sin embargo, para nuestro primer proyecto de dron eso no es lo que queremos. Queremos una aeronave donde al no manipular ninguna palanca, la inclinación en cualquier dirección sea cero. En otras palabras, queremos un dron auto-nivelante. Para lograr esto, debemos calcular los ángulos de inclinación de "roll" y "pitch", como ya hicimos en la sección 5.1 y usar estos datos para compensar el control por velocidad de ambos ángulos. De nuevo, ya veremos la implicación de esto al desarrollar el código.

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;
}
          





Paso 8: Consideraciones adicionales en el programa

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:

  1. Una adición básica pero muy útil es una señal de indicación. Para ello, como ya mencionamos incluiremos un LED con su respectiva resistencia a uno de los pines del Arduino (en este caso, el 2). Usaremos esta señal para indicar cuando el dron esté en modo espera (luz encendida) o en modo operativo (luz apagada). Igualmente el LED nos avisará de algún problema de comunicaciones o de batería baja.
  2. Para medir el nivel de la batería es necesario usar una entrada analógica del Arduino. El microcontrolador del Arduino incluye un periférico llamado ADC (Conversor Analógico-Digital) por medio del cual puede convertir este voltaje analógico en uno de sus pines dedicados para tal efecto en un valor digital. Hay que tener en cuenta que el voltaje de entrada está limitado entre 0 y 5 voltios. Por tanto, como ya mencionamos ligeramente, es necesario diseñar e implementar externamente un "divisor de voltaje", que es simplemente un arreglo de resistencias que convierte proporcionalmente el valor de entrada en un valor de voltaje apto para el Arduino. El valor de entrada es el voltaje de la batería, que en el caso de una pila de Litio-ion de 3 celdas tendrá un valor máximo de 12.4 V. El voltaje mínimo recomendable para operar la batería es de unos 10 V, así que debemos operar en este rango. En el código debemos configurar uno de los pines del arduino como "entrada analógica" y leer continuamente su valor para asegurar que esté dentro del rango permitido.
  3. Otro elemento útil es tener métodos de arranque y parada seguros. No queremos que los motores se enciendan inmediatamente conectamos la batería, así como es necesario tener una parada "de emergencia" en caso de que algo salga mal. Hay miles de opciones para implementar esta funcionalidad; en este ejercicio hemos escogido hacerlo a través de una variable ("start"), que nos indica en qué estado está actualmente la aeronave. Definimos tres posibles estados: 0 - parado, 1 - arrancando, 2 - en operación. Establecemos un método a través del mando remoto para entrar en cada estado: por defecto estaremos en 0, para pasar a 1 debemos hacer una combinación con la palanca izquierda de tirarla hacia la esquina inferior izquierda (esto se traduce en potencia mínima - 1000 - y "yaw" máximo hacia la izquierda - 1000). Cuando soltemos la palanca y el "yaw" vuelva a la posición de centrado, el dron estará en estado operativo (2). Para volver a apagar, tiraremos la palanca hacia el extremo inferior derecho (esto se traduce en potencia mínima - 1000 - y "yaw" máximo hacia la derecha - 2000).
  4. Para detectar un posible error o pérdida de comunicaciones hemos implementado un "timer" que actualiza su valor cada vez que el Arduino detecta un pulso entre 1000 y 2000 us en alguno de los pines conectados al receptor. Si se pierde la comunicación, el receptor no emitirá estos pulsos y por tanto el "timer" no se actualizará. Revisaremos el estado de este "timer" en cada loop y si por alguna razón no se ha recibido señal del receptor en los últimos 2 segundos, la nave pasará a estado 0 inmediatamente.
  5. Es aconsejable también hacer uso del "watchdog" de nuestro Arduino. Esta herramienta consiste básicamente en un "timer" interno del microcontrolador que se encarga de reiniciar el programa después de algún tiempo límite en el que no se cumpla una condición. Por ello puede ser inmensamente útil para evitar "bloqueos" en el programa durante su ejecución. En este enlace puedes ver una sencilla explicación del tema. La implementación del "watchdog" en nuestro programa solo toma tres líneas simples de código para inicializarlo primero y luego reiniciarlo al comienzo de cada ciclo, además de la línea necesaria para incluir la librería que permite su uso.

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.



Paso 9: Descarga del programa, puesta a punto y pruebas

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.



Paso 10: Solución de problemas

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:

  • ¿Están todas las conexiones bien? Se debe verificar la alimentación de todos los componentes y la continuidad en las señales. Un solo cable defectuoso en un componente esencial puede estropear todo el proceso. Reitero, un multímetro viene bien para este proceso.
  • Si no es problema de conexiones, ¿cómo saber dónde está? En el paso 5 hemos visto cómo se integra cada componente al Arduino. Sería recomendable aislar cada componente y repetir estas pruebas individualmente para cada elemento, para verificar que está operando como debe. En este caso el monitor serial de la IDE de Arduino nos es de gran utilidad. Por ejemplo, al verificar la IMU podemos inclinar el dron en todas las direcciones y observar si se obtiene un ángulo en la dirección adecuada que se ajusta a la realidad. Al verificar el módulo de comunicaciones se debe comprobar si se está recibiendo un valor entre 1000 y 2000 en cada canal según la palanca que estemos manipulando. Igual para los motores (¡sin rotores durante las pruebas!) se puede ver si su velocidad de rotación es proporcional al valor ingresado y si el sentido de rotación es el esperado. Para el sistema de control en general es un poco más complicado, pues todo debe estar montado, por lo que lo importante es hacerlo en un ambiente seguro y variar los parámetros de control comenzando por valores pequeños e incrementándolos lentamente.
  • ¿Algún otro problema? Conviene revisar los videos y enlaces auxiliares provistos durante este tutorial, o en su defecto escribir en el foro, donde se tratará de dar solución.


Mira acá el video correspondiente a este paso del tutorial.