Академический Документы
Профессиональный Документы
Культура Документы
Facultad de Ingeniera
Escuela de Ingeniera Elctrica
Por:
Por:
_________________________________
Ing. Fabin Abarca Caldern
Profesor Gua
_________________________________ _________________________________
Ing. Fabin Garita Gonzales Dr. Lucky Lochi Yu Lo
Ingeniero lector Profesor lector
ii
DEDICATORIA
A mi familia y seres queridos, por el constante apoyo y ayuda brindada durante los aos de
mi carrera universitaria, sin la cual no hubiese podido lograr culminar esta etapa de manera
exitosa.
iii
RECONOCIMIENTOS
A mi profesor gua, el Ing. Fabin Abarca, por su apoyo y consejo a lo largo del
iv
NDICE GENERAL
v
2.4.2 Acelermetro ............................................................................................................ 27
UAV 47
BIBLIOGRAFA ............................................................................................ 72
APNDICES ................................................................................................... 76
vii
NDICE DE FIGURAS
Images. .................................................................................................................................. 30
.............................................................................................................................................. 32
Arduino. ................................................................................................................................ 53
Figura 26. Comparacin entre seal original y seal filtrada de uno de los acelermetros. 57
Figura 27. Prueba para observar el desfase entre la seal filtrada y la original, con los
Figura 28. Prueba realizada para observar la inmunidad del sistema ante seales no
deseadas. ............................................................................................................................... 58
Figura 29. Experimento para la obtencin de los parmetro de PID de inclinacin Eje X .. 61
Figura 30. Modelo tridimensional del primer prototipo del cuadracptero. ......................... 65
Figura 31. Modelo Real del Primer Prototipo del Cuadracptero. ....................................... 65
Figura 32. Modelo Real del Primer Prototipo del Cuadracptero. ....................................... 66
Figura 33. Parte inferior del primer prototipo del cuadracptero. ........................................ 66
ix
NDICE DE TABLAS
x
NOMENCLATURA
Controller)
PWM: Modulacin por ancho de pulso (del ingls Pulse Width Modulation).
xi
RESUMEN
inalmbrica va bluetooth con un telfono inteligente que serva de control remoto para el
usuario. Todos los bloques funcionales del cuadracptero fueron validados previamente a
ser integrados al sistema. Este desarrollo se realiz utilizando hardware y software libre, y
volar pequeas distancias, sin embargo, a la hora de concluir el proyecto se determin que
implementarlos. Esto con la finalidad de lograr obtener una mayor robustez y seguridad a la
xii
1. CAPTULO 1: Introduccin
autnoma data desde la era clsica. Sin embargo, no fue sino hasta el siglo XIX, junto con
siglo XX, empez a crecer de manera exponencial, impulsado principalmente por los
El rea de la aeronutica actualmente constituye una de las ramas con ms presencia dentro
Vehicles) desarrollados para una gran diversidad de tareas, que van desde aplicaciones
militares, hasta equipos de investigacin cientfica. Esta constituye el rea que se pretende
Para el desarrollo del presente proyecto, se pretende elaborar un vehculo areo de vuelo
autnomo (UAV), y para ello se busc una plataforma verstil y de construccin sencilla.
Tras realizar una investigacin previa, se eligi tomar como modelo base un quad-rotor, o
tambin conocido como cuadracptero, el cual consiste, como se pude intuir de su nombre,
en una plataforma con cuatro rotores ubicados en los extremos de los ejes de simetra de su
Para su construccin se pretende hacer uso de hardware open source (Arduino) para el
Dentro de las principales razones por las cuales se toma la decisin de abordar un proyecto
desarrollo de un prototipo de esta categora, y dado que este proyecto se encuentra ligado a
la finalizacin del grado acadmico, permite la oportunidad de aplicar y demostrar una gran
cantidad de conocimientos que se han adquirido a travs de la carrera. Entre las reas de
2
1.1 Objetivos
3
1.2 Metodologa
La metodologa que se implement para el desarrollo del proyecto fue una extensa
investigacin de los componentes necesarios (tanto fsicos como tericos) para realizar el
desarrollo del sistema de manera exitosa. Adems, cada uno de los sistemas estudiados,
para poder llegar a obtener el comportamiento esperado de cada uno de los subsistemas.
El cronograma original del proyecto a desarrollar fue evolucionando con el transcurso del
4
2 CAPTULO II: Desarrollo Terico
En el desarrollo del captulo II, se sintetizarn los temas de mayor importancia para el
prototipo del cuadracptero. Estos temas se tratarn de exponer de una manera sencilla para
que sea apto para lectores con o sin mayor conocimiento tcnico del tema.
2.1 Aerodinmica
Con la finalidad de ir desarrollando los temas en un orden estructurado, se iniciar con una
breve seccin de aerodinmica, la cual a grandes rasgos, constituye la base que permite
explicarn brevemente los conceptos aerodinmicos de mayor relevancia para crear la base
Uno de los conceptos fundamentales que surge cuando se empieza a estudiar el campo de la
fuerza perpendicular al flujo de un fluido (en nuestro caso el aire) que se crea cuando el
fluido pasa por la superficie de un objeto. El paso del fluido por la superficie del objeto crea
componente vertical al vector de direccin del fluido. Dentro de los otros componentes de
esta fuerza creada por el paso del fluido, tambin se encuentra la fuerza de arrastre, la cual
5
tambin juega un papel muy importante en la dinmica de operacin de los vehculos
areos.
En el caso de los vehculos que poseen aspas adheridas a rotores, sta fuerza es la
encargada de crear un par rotacional en el vehculo cuya direccin va en contra del sentido
de giro de las aspas. Por esta razn es que los helicpteros convencionales poseen rotores
de cola que compensan este par rotacional y evita que giren sin control alrededor del eje de
El ngulo de ataque de un ala se encuentra definido como el ngulo que forma la cuerda
geomtrica de un perfil alar con la direccin del aire que incide sobre el ala. Este parmetro
arrastre de una hlice. Para un mejor entendimiento de este concepto se puede observar la
figura a continuacin.
La correcta eleccin del ngulo de ataque de las hlices es de suma importancia para
2.2.1 Introduccin
Los vehculos areos no tripulados (popularmente conocidos como UAVs por sus siglas en
ingls), son vehculos areos que estn diseados para poder volar sin necesidad de un
por un piloto humano que se encuentra en un centro de mando en tierra, desde donde puede
transmitirle las instrucciones de vuelo de manera remota, o tambin, stas mquinas pueden
Dentro de la gama de vehculos areos no tripulados, se puede encontrar una gran escala de
tamaos , configuraciones y formas (vehculos de ala fija, vehculos multirotores, etc), los
piloto, pero que en los ltimos aos, gracias a los grandes avances de la tecnologa en el
dependiendo de las caractersticas de cada uno, stos se pueden construir como vehculos
encuentra formado (como su nombre lo dice) por cuatro rotores, los cuales normalmente se
encuentran dispuestos simtricamente a en los extremos del bastidor del vehculo (el cual es
cuadracpteros poseen hlices con aspas fijas, en contraste con los helicpteros
continuacin:
8
Dinmica de Movimiento
movimientos bsicos, los cuales se pueden ejecutar por s solos o en conjunto para lograr el
tipo de movimiento especfico que el controlador del vehculo desee. Estos tipos de
Para la ilustracin de este movimiento, primero se procede con la definicin del eje z del
vehculo. Este eje imaginario, se encuentra en el centro del bastidor de la mquina, (el
rotacin de las aspas crean un torque que tratan de girar el vehculo alrededor del eje de
giro de las aspas. En los helicpteros convencionales, este problema se corrige mediante un
rotor en su cola que genere una fuerza tangente al radio de giro del rotor principal, la cual
sea suficiente para crear un par opuesto que contrarreste el efecto del par generado por el
rotor principal.
En un cuadracptero no se cuenta con rotores de cola (ni colas en s), de modo que este
problema se soluciona de una manera distinta (la cual resulta muy ingeniosa). Como se
9
sabe, los cuadracpteros poseen un nmero par de rotores, por lo tanto se puede tener dos
rotores girando en un sentido, y dos rotores girando en otro sentido. De esta manera, al
operacin donde el par rotacional neto del vehculo sea igual a cero.
Segn lo expuesto en [5], se puede concluir que tanto la fuerza de sustentacin como la
fuerza de arrastre son funciones que dependen del cuadrado de la velocidad angular. A
Ecuacin 2.1
Ecuacin 2.2
Donde son parmetros propios del fluido (aire) y del tipo de hlices y
constantes el tipo de fluido donde se est volando, y el tipo de hlices que se estn
utilizando, la nica variable que queda es la velocidad angular. Por esta razn, el par
rotacional creado por la propela de cada motor puede ser conocido y modificado de acuerdo
10
Ahora, como lo que se trata de controlar es la rotacin alrededor del eje z del
cuadracptero, basta con igualar los pares rotacionales creados por los motores que giran en
contra de las manecillas del reloj, y los pares rotacionales creados por los motores que giran
a favor de las manecillas del reloj (en caso de que se quiera que el cuadracptero no gire
alrededor del eje z). Esto lo logramos controlando las velocidades netas de cada uno de los
sentidos de giro (partiendo del hecho que las hlices sean simtricas y con las mismas
caractersticas constructivas).
Sean y las velocidades angulares de los motores girando a favor de las manecillas
del reloj y y las velocidades angulares de los motores girando en contra de las
quiera girar. Para realizar esta rotacin lo que se realiza es disminuir/aumentar la velocidad
de rotacin de uno de los motores y realizar la accin inversa en el motor que se encuentre
al lado opuesto del eje. Cabe resaltar que la velocidad neta (ya sea en contra o a favor de
reloj) de los motores debe permanecer constante en caso de que no se quiera que el
A continuacin se puede observar la figura que ilustra el caso anterior para que se visualice
mantener la velocidad neta de rotacin a favor de las manecillas del reloj constante.
La dinmica resulta la misma para el caso anlogo de la rotacin alrededor del eje Y.
13
Una consecuencia de provocar una rotacin alrededor de alguno de los ejes X o Y, es que
una traslacin a lo largo del eje Y, y si el cuadracptero rota alrededor del eje Y, se va a
obtener una traslacin a lo largo del eje X. Cabe resaltar que tambin se puede obtener una
rotacin simultnea en ambos ejes, lo cual va a provocar una traslacin anloga a esa
rotacin.
Control de Altitud
El control de altitud resulta de una manera muy intuitiva. Se parte de que la altitud se
Para realizar el control de altitud, basta con aumentar o disminuir de manera equivalente las
velocidades de los cuatro motores del cuadracptero. Si se quiere ganar altitud, se procede a
aumentar la velocidad de todos los motores en un factor equivalente a la rapidez con la que
14
A continuacin se encuentra el esquema que ilustra el proceso descrito:
Resulta fcil llegar a la conclusin de que con una determinada combinacin de cualquiera
de los tres tipos de movimientos descritos anteriormente se puede obtener un control total
velocidad final de cada uno de los motores va a ser la suma dinmica de cada uno de las
velocidades necesarias para lograr cada uno de los tres tipos de movimiento que se estn
15
2.3 Plataforma de Prototipos Electrnicos: Arduino
Arduino ha surgido en los ltimos aos como una plataforma de creacin de prototipos
movimiento de Hgalo usted mismo (Do It Yourself). Esto se debido a su gran facilidad,
poca experiencia y conocimiento del tema para iniciar con el desarrollo de prototipos.
Arduino nace como una plataforma que puede percibir su entorno mediante una gran gama
de sensores, y puede afectar sus alrededores controlando motores, luces y otra gran
con Arduino pueden ser utilizados de manera independiente, o pueden comunicarse con
La familia de Arduino cuenta con una gran variedad de integrantes (tipos de tarjetas
distintas), cada una con su forma y caractersticas propias. Esto resulta muy prctico para
seleccionar una que se adapte lo mejor posible a las caractersticas de cada proyecto.
16
2.3.2 Hardware del Arduino Uno
Dado que para el presente proyecto se decidi utilizar el Arduino Uno, se va a profundizar
utilizar como salidas PWM (seales de modulacin por ancho de pulso). Adems, posee 6
entradas analgicas, un oscilador de 16 MHz, un conector USB para comunicacin con una
17
A continuacin se presenta una tabla resumen con las principales caractersticas del
Arduino Uno:
Fuente de Poder
El Arduino Uno puede ser alimentado desde su conector USB o mediante una fuente de
poder externa, como por ejemplo una batera. La fuente de poder es seleccionada de manera
18
automtica por el dispositivo, y se puede acoplar al Arduino por medio de su conector de
La tensin de alimentacin de la tarjeta puede variar entre 6 y 20 volts, sin embargo cuando
suplir los 5 volts deseados (que se obtienen a travs del pin de 5V). Adems, cuando se usa
sobrecargue y se dae. Por las razones anteriores, la tensin de entrada recomendada para el
Arduino es de 7 a 12 volts.
Vin: Es el pin de entrada cuando se est utilizando una fuente de poder externa (no
3.3V: Es la salida del regulador de 3.3 volts de la tarjeta utilizado para sensores y
19
Memoria
El ATmega 328 posee 32 kB de memoria, de los cuales 0.5 kB son utilizados por el
pueden ser accesados o modificados mediante una de las libreras destinadas para tal fin.
Entradas y Salidas
El Arduino Uno posee 14 pines digitales, los cuales pueden ser utilizados como entradas o
salidas. Estos pines trabajan a 5 volts, y pueden entregar un mximo de 40 mA. Adems,
Pines Seriales: 0 (RX) y 1 (TX). Son utilizados para recibir y transmitir datos
seriales TTL. Estos pines estn conectados a los pines correspondientes del chip
Pines de PWM: 3,5,6,9,10 y 11. Proveen una salida PWM de 8 bits de resolucin
20
El Arduino Uno tambin posee 6 entradas analgicas con un convertidor analgico-digital
de 10 bits de resolucin. Por default, esos convertidores miden desde 0 hasta 5 volts, pero
analogReference().
2.3.3 Programacin
Arduino. Los programas que se realizan para Arduino son denominados Sketch. Una vez
cargarlo al Arduino que se tenga conectado al computador. Para un mayor detalle de las
cantidad de libreras realizadas por sus usuarios, las cuales pueden ser descargadas de
manera gratuita para su respectivo uso. A continuacin se muestra una imagen del entorno
de programacin:
21
Figura 8. Entorno de programacin de Arduino (programa de ejemplo).
2.4 Sensores
Las palabras sensor y transductor son dos trminos muy utilizados para referirse a
donde se lleva a cabo un cambio entre tipos de energa. Un ejemplo de estos sistemas son
mecnico se manifiesta como una salida de tensin elctrica en el dispositivo. Por otro lado
22
el trmino sensor es utilizado principalmente para referirse a dispositivos que responden
ante algn estmulo del ambiente en el que se encuentran cambiando algn parmetro
intrnseco del dispositivo, pero sin realizar ninguna conversin de energa. Un ejemplo de
sensores son los termistores, los cuales son dispositivos que simplemente cambian el valor
pequeo de la variable medida que el transductor puede detectar. Estos parmetros resultan
electromecnicos, o MEMS por sus siglas en ingls, los cuales son la base de los
23
2.4.1 Giroscopio
Los giroscopios son dispositivos utilizados para medir o mantener una orientacin
especfica. Antiguamente solo se contaba con los giroscopios mecnicos, que consistan en
discos giratorios suspendidos sobre un eje que poda rotar en varias direcciones. La
Actualmente se cuenta con otro tipo de giroscopios, los cuales son basados en sistemas
debido a su reducido tamao, bajo precio y buena precisin. Una de las mayores
caso de los vehculos areos no tripulados (UAV) para sus sistemas de estabilizacin.
El IDG-500 es un giroscopio de dos ejes que consiste en dos girscopos MEMS vibratorios
independientes. Uno de ellos detecta la velocidad de rotacin alrededor del eje X, y el otro
24
selladas hermticamente al nivel de la oblea de silicio. Las masas vibratorias de ambos
control automtico muy preciso que controla la oscilacin de las masas. Cuando el sensor
es rotado alrededor del eje X o el eje Y, el efecto Coriolis causa una vibracin que puede
filtrada para dar paso a una seal analgica que es proporcional a la tasa de cambio de la
A continuacin se puede observar un diagrama de Bode del filtro paso bajo que posee el
25
El IDG-500 posee dos salidas de velocidad para el eje X, y dos para el eje Y. A
26
2.4.2 Acelermetro
Analog Devices. Este dispositivo tiene un rango de medicin de 3g. Contiene un sensor
sistema puede medir aceleracin esttica (como la gravedad), o aceleracin dinmica (como
oblea de silicio. Posee resortes de silicio que suspenden una estructura sobre la superficie
diferencial que consiste en una placa fija en la oblea y otra placa adherida a la masa
suspendida. El sensor utiliza una sola estructura para la medicin de la aceleracin de los
27
A continuacin se puede observar un diagrama de bloques funcionales del sistema de
medicin:
Seguidamente, tambin se muestra una tabla con los parmetros de mayor importancia para
28
Tabla 4. Especificaciones de Acelermetro ADXL335
Los sensores ultrasnicos de proximidad (que en realidad son transductores pero son
conocidos popularmente como sensores), trabajan bajo un principio muy similar a un radar,
radio. Los sensores ultrasnicos generan ondas de alta frecuencia, detectan el eco, y
el que se recibi el eco, para determinar la distancia del objeto que se est avaluando. Esta
29
del material del objeto que se est midiendo, dado que determinados materiales pueden dar
Figura 11. Diagrama de Operacin de Sensor Ultrasnico de Proximidad. Fuente: Google Images.
El sensor ultrasnico de proximidad que se eligi para la realizacin del proyecto es el HC-
SR04, de la compaa ITead Studios. A continuacin se puede apreciar una imagen del
sensor:
transductor enva una seal ultrasnica que se encuentra compuesta de 8 pulsos de 40 kHz,
la cual eventualmente ser reflejada por un objeto (en caso de que lo hubiese). A esta seal
reflejada se le denomina eco. Desde el momento en el que se emite la seal de 40 kHz hasta
ECO. Cuando el transductor recibe de vuelta esta seal la convierte en una seal
elctrica, y el siguiente pulso no podr ser transmitido hasta que el eco de la seal anterior
haya desaparecido. Este tiempo que transcurre entre el momento en que se enva el primer
pulso ultrasnico hasta que se puede enviar el prximo se denomina periodo de ciclo (cycle
transductor es proporcional al tiempo que dure la seal de la terminal de ECO en alto (que
en este caso es una tensin de 5 volts). Si no se detecta ningn obstculo frente al sensor
La expresin que se utiliza para calcular la distancia en centmetros del objeto (o superficie,
31
Para un mejor entendimiento, a continuacin tambin se brinda una explicacin grfica de
su operacin:
2.5 Actuadores
Los motores DC sin escobillas (tambin conocidos como motores DC brushless), son
motores sncronos que son alimentados por una fuente elctrica DC. Sin embargo, a
diferencia de los motores DC con escobillas, los motores sin escobillas requieren de un
conmutador electrnico y sensores adicionales para generar una seal de control que
energice cada una de las bobinas del motor en forma secuencial para dar paso a la rotacin
32
del motor. La gran mayora de estos motores se encuentran dentro de la categora de
escobillas, como por ejemplo, la obvia ausencia de escobillas, que representan una gran
desventaja debido a su desgaste y mantenimiento. Adems los motores sin escobillas son
presentan eficiencias y pares superiores por cada watt consumido. Una caracterstica muy
producen chispa alguna cuando se encuentran trabajando. Esto hace a los motores DC sin
Una de las desventajas de los motores DC sin escobillas, es que requieren de un sistema
electrnico por aparte para poder operar, normalmente denominado como controlador
electrnico de velocidad (ESC por sus siglas en ingls), y del cual se hablar ms adelante.
El motor DC con escobillas puede operar sin problemas conectado nicamente a una fuente
cual indica la cantidad de revoluciones por minuto a la que gira un motor por cada volt de
33
alimentacin. Por ejemplo, un motor sin escobillas de 1000 Kv conectado a una fuente de 2
Tpicamente los motores DC sin escobillas poseen imanes permanentes adheridos a una
armadura fija, eliminando as los problemas generados para suplir corriente a una armadura
en los motores con escobillas, y ste se encarga de cambiar continuamente cada una de las
fases del motor para generar un campo magntico rotativo que se encargue de inducir el
Las bobinas de los motores sin escobillas se pueden encontrar tanto en configuraciones
Delta (las cuales entregan menor torque a bajas velocidades, pero generan mayores
velocidades) como configuraciones Estrella (que generan mayor torque a bajas velocidades
34
Figura 14. Representacin de Motor DC sin Escobillas
Los Controladores Electrnicos de Velocidad (ESC por sus siglas en ingls) son los
dispositivos encargados de coordinar y dar paso a la rotacin y control de los motores sin
escobillas. Estas unidades se encargan de generar la secuencia de pulsos trifsicos que son
Para controlar la rotacin del motor, el controlador requiere de un medio para determinar la
orientacin del rotor con respecto al estator. Para este fin, algunos diseos utilizan sensores
de efecto Hall (los cuales detectan el campo magntico del rotor) y otros mtodos detectan
adicionales, y por ende este tipo de sistemas son llamados controladores sin sensores.
35
Los controladores tpicos poseen 3 salidas bidireccionales, las cuales popularmente
trabajado el controlador. Para realizar el proceso anterior, normalmente los ESC tienen que
ser configurados con los parmetros del sistema en el cual estn trabajando (como por
ejemplo el tipo de bateras al cual est conectado, las tensiones mnimas de operacin
El reto al que se enfrentan los controladores que infieren la posicin del rotor con respecto
a la fuerza contra electromotriz de las bobinas que no estn en uso, es a la hora de iniciar el
movimiento, ya que no hay una fuerza contra electromotriz (el rotor se encuentra esttico, y
por ende no hay tensin inducida). Usualmente, el procedimiento que se sigue es iniciar el
movimiento desde una fase arbitraria, y luego corregirla en caso de que no sea la correcta.
motores sin escobillas trifsicos. Consiste en un motor de 3 bobinas por fase, cuyo rotor es
las fases A, B y C son generados por el ESC. Para este ejemplo en particular, se necesita
repetir 6 veces una secuencia de 6 combinaciones distintas de pulsos para lograr una
una manera secuencial por medio de algn canal de comunicacin. Este tipo de
comunicaciones se realiza cuando el costo de tener varios canales es muy alto, o cuando
37
de las comunicaciones seriales, es que poseen tasas de transmisin de datos ms bajas que
las comunicaciones en paralelo, dado que se pueden transmitir menos datos por unidad de
tiempo, sin embargo, resulta ventajoso para reducir costos cuando la velocidad de
Como ejemplos de los buses ms conocidos que implementan comunicacin de tipo serial
se pueden mencionar: SPI, I2C (ambas pueden ser implementadas mediante el Arduino),
datos de manera serial a distancias cortas mediante radiofrecuencias que van desde los 2400
MHz hasta los 2480 MHz. Este protocolo fue creado por la empresa de telecomunicaciones
Ericsson en 1994, el cual fue concebido originalmente como una alternativa inalmbrica a
Este protocolo est diseado para comunicar dos o ms dispositivos al mismo tiempo, los
38
El protocolo Bluetooth se encuentra regido por el Bluetooth Special Interest Group o SIG
por sus siglas en ingls, el cual es un grupo formado por ms de diecisis mil compaas de
encarga de velar por que se cumplan los estndares requeridos para los nuevos dispositivos
riesgos de seguridad, debido a la exposicin de los datos que se transmiten. Para evitar
exponer datos privados a receptores no deseados, este protocolo cuenta con un proceso de
dispositivos que quiere comunicar entre s, y una vez realizado el proceso, los dispositivos
pueden seguir comunicndose entre s con la seguridad del conocimiento del usuario de
donde se estn transmitiendo los datos. Una vez realizado el emparejamiento, los
informacin entre s.
39
2.7 Procesamiento de Seales
2.7.1 Introduccin
percibir el entorno y el estado del cuadracptero, stos proporcionan seales de salida que
se van a ver sometidas a ciertos niveles de ruido (causado tanto por vibraciones mecnicas
como por interferencias electromagticas). Por esta razn, resulta de gran importancia para
el adecuado control del vehculo, acondicionar estas seales de manera que se puedan
eliminar los datos provenientes de los sensores que no sean provechosos para el control del
elctrica), sin embargo, debido al alcance de este proyecto y las limitaciones de tiempo y
tiempo real.
40
2.7.2 Manipulacin de los datos de los sensores (IMU)
Como una tcnica para disminuir el nivel de ruido propio de los acelermetros, y disminuir
el efecto del deslizamiento de las medidas de los giroscopios, se puede hacer uso de una
fusin de las mediciones tomadas de ambos transductores. Dado que se puede obtener una
medida del desplazamiento angular por medio de ambos sensores, entonces, al asignar
pesos dinmicos que sean relativos a la confiabilidad de las medidas de cada uno de los
sensores, se puede obtener una medida ms robusta del desplazamiento angular real del
En estadstica, una media mvil, es un tipo de filtro utilizado para analizar grupos de datos
creando una serie de promedios de diferentes subgrupos del grupo global de datos. La
Dada una serie de nmeros y un tamao de subgrupo fijo, el primer elemento de la media
mvil es obtenido sacando el promedio de los valores del primer subgrupo de la serie de
serie, y reemplazndolo por el siguiente nmero de la serie original de datos. Esto genera
41
un nuevo subgrupo de datos, el cual a su vez tiene un valor de promedio numrico nuevo.
Este proceso es repetido hasta abarcar todos los datos de la serie numrica original. A la
grfica que conecta los valores de cada una de las medias obtenidas se le denomina media
mvil.
Las medias mviles tambin pueden ser utilizadas agregando pesos distintos a los valores
que son de mayor inters, para que estos tengan mayor o menor impacto sobre el promedio
42
Dado un grupo de 8 datos, y un tamao de subgrupo de 3 datos, se procede a sacar el
promedio de los tres primeros datos. Luego el subgrupo se desplaza una casilla hacia la
izquierda y se obtiene de nuevo el promedio del subgrupo. Este proceso se repite hasta
llegar al final de los datos, o en el caso de que sea un proceso que genera datos
continuamente, hasta que termine el proceso (como lo ser nuestro caso). Seguidamente se
observa un grfico lineal donde se pueden observar los datos originales del ejemplo, y los
de la media mvil. Se puede observar como la media mvil sigue la misma tendencia de los
datos originales, solamente que se ve menos afectada por los picos que existan en la serie
de datos original.
43
Esta es una de las principales utilidades de la media mvil, el atenuar las fluctuaciones de
pequea duracin de los datos originales, y darles ms importancia a las tendencias de largo
plazo. La diferencia entre largo y corto plazo depende de cada aplicacin, por ende, el
tamao de los subgrupos, y el peso de los valores (en caso de que se requieran agregar) se
La media mvil tambin se puede ver como un tipo de filtro paso bajo, donde los datos
aleatorios de alta frecuencia pueden ser atenuados por medio del promediado de valores
para obtener una tendencia ms adecuada y eliminar el ruido del sistema. Cabe resaltar que
dado que mientras ms se trate de atenuar el ruido, ms lenta ser la respuesta del sistema.
Los controladores PID son sistemas que trabajan bajo el principio de realimentacin de lazo
cerrado, los cuales se encargan de manipular una o ms variables de un sistema (las cuales
44
se trabajan como entradas del sistema) con la finalidad de reducir la diferencia existente
entre la salida real del sistema y la salida deseada del sistema (conocido como el error).
Los algoritmos de control PID poseen 3 componentes cuya suma da resultado al parmetro
sistema, es lo que da paso a lograr una respuesta de la salida del sistema en la que se
posible.
La ecuacin generalizada de los controladores PID viene dada por la siguiente expresin:
Donde:
Litio o LiPo por su abreviatura en ingls, son un tipo de bateras recargables de alto
rendimiento, las cuales normalmente se encuentran compuestas de una o varias celdas que
caracterizan porque permiten niveles altos de descargas de corriente elctrica, motivo por el
cual, estas sern las bateras seleccionadas para el desarrollo del presente cuadracptero.
46
3. CAPTULO III: Desarrollo e Implementacin del UAV
proceder con el modelado del cuadracptero por desarrollar. Cada uno de los bloques
representa un subsistema que ser diseado y validado por aparte, para luego formar parte
de todo el sistema. Esto permite segmentar el proyecto en diversas etapas las cuales hacen
al desarrollador.
Mdulo de
Sensado
Fuente de
Alimentacion
47
Cabe aclarar que el cuadracptero que se construir, se disear en configuracin +. A
continuacin se detalla visualmente los dos tipos de configuraciones posibles para el diseo
de cuadracpteros:
ms sencilla aplicacin.
A continuacin se expone el diagrama de bloques funcional para cada uno de los motores.
La velocidad final de cada uno de ellos, estar constituida por la suma de la salida del PID
de control de altitud, el PID de control de rotacin alrededor del eje X (o Y), y un valor de
velocidad fijo para iniciar con una velocidad conocida cuando se encienda el
cuadracptero. El resultado de dicha suma (la cual se convertir en una seal PWM por el
correspondientes para lograr la velocidad angular requerida para cada uno de los motores.
Motor X
Entrada de Seales de
Transductores
PID Altitud
Salida de Velocidad de
S
Motores
PID Rotacion Eje X Velocidad Motor X
Valor Fijo
49
Figura 22. Diagrama de bloques funcionales para cada motor
lazo de realimentacin y ejecucin. Cada uno de los bloques de los motores se debe
figura, la posicin y postura del cuadracptero va a estar determinada por la interaccin del
comportamiento de cada uno de los cuatro motores. Esta postura va a ser traducida en un
conjunto de seales elctricas por los transductores, las cuales van a ser introducidas en los
Motor 1
Motor 2
Transductores de
Posicin y Postura
S Posicin y Postura
Deseada
Motor 3
Motor 4
50
La tabla de asignacin de entradas y salidas del Arduino ser como se muestra a
continuacin. Cabe resaltar que hay un pin de entrada analgica libre, sin embargo,
prximamente este ser destinado para la salida del eje Z de un nuevo giroscopio. Este
nuevo eje del giroscopio ser requerido para la correccin de la rotacin alrededor del eje Z
del cuadracptero, la cual es causada por la diferencia de los pares rotacionales entre los
motores CW y los motores CCW. Esta tabla se propuso de esta manera dado que el
giroscopio con el cual se cuenta actualmente solo posee dos grados de libertad.
Como se observa, se tienen seis entradas/salidas digitales libres, las cuales se pueden
51
3.2 Experimentos
La validacin de este mdulo se realiz de una manera muy sencilla. Primero se procedi a
dispositivo.
Luego, se realiz un sketch sencillo para el Arduino, el cual permitiera observar que se
consisti en utilizar 5 leds de distintos colores que se encontraban conectados a las salidas
manera remota por medio del adaptador bluetooth utilizado en conjunto con el
microcontrolador Arduino.
52
Figura 24. Diagrama de mtodo de validacin de mdulo bluetooth en conjunto con Arduino.
transmisor en otro cuarto distinto. Sumado a la prueba visual realizada mediante los LEDs,
el Arduino y el computador por medio del monitor serial de datos que posee la misma
aplicacin con la cual se realiza la programacin del Arduino (se puede observar una
siguiente manera:
Cada ESC debe ser programado antes de su primera utilizacin. Esto debido a que
requieren de diversos parmetros propios del sistema donde van a ser utilizados para un
proveedor.
validacin en conjunto con el Arduino Uno. Para esto se realiz un pequeo sketch
exclusivo para el control de velocidad de los motores. A grandes rasgos, el concepto del
experimento de validacin fue utilizar una seal analgica de entrada, generada mediante
generada mediante el potencimetro. Los cuatro pines de control de cada uno de los
54
Arduino, y la tensin variable del potencimetro se coloc en una de las entradas
de los motores se pudo llevar a cabo mediante la seal analgica de entrada que fue
Este apartado correspondi a uno de los mayores retos del desarrollo del proyecto. Como se
ruido que va en detrimento de la robustez del control automtico del cuadracptero. Este
ruido es dependiente del tipo de componentes mecnicos que se estn utilizando (como por
ejemplo las caractersticas de las hlices, el material del marco, etc). Por esta razn se tuvo
que implementar una etapa de filtrado digital de las seales para tratar de eliminar la mayor
Para lograr esto, se implement la tcnica de la media mvil para realizar un promediado
dinmico de los valores de cada una de estas seales, de modo que se disminuyera el nivel
de ruido, sin desfasar en gran medida la seal filtrada de la seal real. Esto es de suma
importancia debido a que los controladores PID requieren obtener su seal de entrada en
55
El parmetro requerido para este filtro (cantidad de muestras que se promedian en cada
muestreo de las seales de los acelermetros y giroscopio a 100 Hz, mientras que la
debido a que cada muestreo de este sensor puede durar hasta 25 ms (vara dependiendo de
la distancia del objeto o superficie), de modo que si se realizara a la misma frecuencia que
A continuacin se observa un grfico que muestra la seal sin filtrar (en azul) junto con la
seal filtrada (rojo). Esta captura fue tomada con los motores encendidos a un 15% de su
velocidad mxima. Se puede apreciar la gran reduccin del ruido que se logr al realizar la
fusin de las seales de los acelermetros y los giroscopios, junto con el promediado
por ciclo.
56
Figura 26. Comparacin entre seal original y seal filtrada de uno de los acelermetros.
Adems se puede observar el desfase que se obtiene entre la seal filtrada (roja) y la seal
sin procesar (azul). Para este experimento se conservaron los mismos parmetros de la
prueba anterior, solamente que se apagaron los motores para observar una seal ms limpia
de los acelermetros. Tambin cabe mencionar que para estas pruebas se utiliz un peso
relativo del 80% para la seal del acelermetro y un peso de 20% para la seal del
giroscopio. Se observ que mientras mayor peso se le asigne a las lecturas del giroscopio,
Figura 27. Prueba para observar el desfase entre la seal filtrada y la original, con los motores
apagados.
57
Por ltimo, se realiz una prueba para observar la inmunidad del sistema ante la presencia
observaron ambas seales de inters (seal filtrada y seal original). Se puede observar que
Figura 28. Prueba realizada para observar la inmunidad del sistema ante seales no deseadas.
de modo que dependiendo de las ganancias de cada una de las componentes, as ser el
impacto en cada una de ellas. En el caso de la parte integral del controlador, no se tiene
mayor problema con el ruido, dado que si este presenta una distribucin gaussiana, no
58
diferencial del controlador, si se ver muy afectada, dado que la pendiente de las seales de
ruido de alta frecuencia es muy pronunciada, motivo por el cual la derivada de estas seales
resulta en una cifra muy alta. Debido a esta razn, la ganancia de la componente derivativa
del controlador se tiene que subdimensionar para lograr que sea inmune a este ruido. El
reacciona de manera adecuada ante cambios bruscos reales en la postura del cuadracptero,
De aqu la importancia de tener una seal bien acondicionada, para poder dimensionar
logar que el cuadracptero responda de una mejor manera ante cambios bruscos en su
postura.
Para la validacin del transductor ultrasnico se decidi utilizar la librera facilitada por el
luego observar si las distancias obtenidas mediante el sensor ultrasnico coincidan con las
medidas reales de distancia (las cuales fueron medidas con una cinta mtrica). Los
resultados fueron los esperados, y las lecturas fueron muy precisas. Cabe resaltar que
mediante la experimentacin se pudieron medir distancias que fueron desde los 2 cm hasta
los 500 cm. Fuera de ese rango, las lecturas eran inestables.
59
El Sensor Ultrasnico de Proximidad se valid con el siguiente sketch:
#include "Ultrasonic.h"
Ultrasonic ultrasonic(12,13); // Trigger en Pin 12 y Echo en Pin 13
void setup() {
Serial.begin(115200);
Serial.print("testing...");
}
void loop()
{
Serial.println
(ultrasonic.Ranging(CM)); //La salida se puede obtener en centmetros (CM) o pulgadas (IN).
delay(500);
}
Para la validacin del sistema de control automtico, primero se procedi a obtener las
ganancias de las componentes de cada uno de los controladores PID. Dado que no se cuenta
con un modelo matemtico del sistema, y la respuesta del sistema a los escalones es un
proceso complejo de obtener sin el equipo necesario, las ganancias de cada PID se
60
Para realizar este proceso, la dinmica que se sigui fue calibrar cada PID por separado
hasta obtener el comportamiento deseado, para luego ponerlos a trabajar todos juntos una
Primero se procedi con la obtencin de los parmetros para el PID de control de rotacin
alrededor del eje X. Para realizar esto, se restringi la rotacin del cuadracptero alrededor
del eje Y al apoyar el eje X en una superficie libre de friccin que lo permitiera rotar. De
esta manera, solo se trabaj con los motores 2 y 4 encendidos, los cuales eran los
permanecen apagados).
Figura 29. Experimento para la obtencin de los parmetro de PID de inclinacin Eje X
Inicialmente, el proceso que se sigui, fue configurar todas las ganancias de este PID en 0.
Luego, poco a poco se fue aumentando la ganancia proporcional del controlador hasta
61
llegar a un punto donde el cuadracptero se encontrara cerca de empezar a oscilar alrededor
horizontal). Ya una vez que se obtuvo el valor de este parmetro, se sigui el mismo
proceso pero para la ganancia diferencial, solo que esta vez con la ganancia proporcional en
Cabe resaltar, que los parmetros encontrados para el sistema con un tipo de hlice, no son
vlidos si se cambian las hlices, de modo que se tienen que volver a obtener los
parmetros.
Luego de obtener los parmetros del PID del Eje X, se procede a realizar el procedimiento
anlogo pero para el controlador PID del Eje Y. Esto debido a que el cuadracptero no es
62
observar ms claramente en el video que se encuentra ingresando en el siguiente link:
http://www.youtube.com/watch?v=-r1t5x4S7XY&feature=plcp.
Para este punto de operacin, las ganancias que se obtuvieron fueron las siguientes:
Los parmetros se obtuvieron con hlices de 8 pulgadas de dimetro. Cabe destacar que la
diferencia entre los parmetros del eje X y del Eje Y se debi principalmente a la forma de
la batera (dado que constituye un 30% del peso total) y como se encontraba dispuesta en el
Este proceso de obtencin de los parmetros se puede facilitar y optimizar al contar con el
escaln de cada uno de los ejes. Sin embargo, por motivo de tiempo y recursos, para la
realizacin del presente proyecto no se pudo contar con las herramientas requeridas. No
obstante, fuera del hecho que la obtencin de los parmetros no se dio de la manera
Adems se aclara que para la experimentacin que se detall anteriormente, se incluy una
proteccin en la salida de los controladores PID, de modo que la salida se sature a cierto
nivel conocido (en nuestro caso un 30% de la velocidad mxima de los motores). Esto para
evitar que por algn error de programacin o algo similar, el cuadracptero se salga de
63
control y ocurra algn accidente. La adicin de todo tipo de protecciones es una prctica
recomendada en caso de que se vaya a experimentar o realizar algn cambio del que no se
A continuacin se puede observar una tabla resumen con los componentes utilizados en la
construccin del prototipo y sus costos correspondientes. Se aclara que el marco se realiz
64
El cdigo de programacin del Arduino que se utiliz para realizar las pruebas se encuentra
en el Anexo2. Una vez que se ensambl por completo el prototipo, el resultado final fue el
siguiente:
66
4. Captulo 4: Conclusiones y Recomendaciones
4.1 Conclusiones
inicialmente.
correctamente.
hlices result ser ms peligroso de lo pensado, dado que los motores se encuentran
algn error de programacin. Se tuvieron que tomar todas las precauciones del caso
67
El Filtro Kalman result tener una complejidad ms alta de lo pensado inicialmente,
motivo por el cual, dadas las limitaciones del alcance del proyecto, no se pudo
pruebas y experimentos que se tienen que llevar a cabo en un lapso de tiempo muy
estable y funcional.
vuelo que no sea asistido por un piloto. Al tener mucho procesamiento matemtico,
programa hasta llegar a un nivel donde es muy bajo para la correcta operacin
objetivo especfico. El Arduino se pudo comunicar sin ningn problema tanto con la
68
computadora personal como con el dispositivo Android para enviar los datos de
postura y otras variables de inters para el usuario por medio del shield bluetooth.
ser un muy buen ejercicio acadmico, sin embargo resulta poco prctico tener que
controlar un sistema tan dinmico y de rpida respuesta por medio de una pantalla
tctil.
69
4.2 Recomendaciones
Se recalca el cuidado que hay que tener al hacer las pruebas con las hlices
utilizar para validaciones. Este es un recurso muy importante dado que permite
70
Documentar muy bien todos los experimentos que se realicen, y los cambios que
Hacer buen uso de los profesores tutores para orientar la resolucin de los
proyecto.
de control ms robusto mediante el uso de este modelo (Como por ejemplo con
Filtros de Kalman).
vuelo.
71
BIBLIOGRAFA
Artculos y Papers:
Design for a Quadrotor Unmanned Air Vehicle. Centre for Research in Applied
4. Paul Pounds, Robert Mahony, Peter Corke. Modelling and Control of a Quad-
5. Jun Li, Yuntang Li. Dynamic Analysis and PID Control for a Quadrotor. 2011
2010.
72
7. Alexandros Soumelidis, Peter Gaspar, Gergely Regula, Bela Lantos. Control of
in PID Controller for Stabilizing Quadrotor Flight Control. 2011 IEEE 7th
Libros:
10. Usher M.J. and Keating D.A. Sensors and Transducers, Segunda Edicin,
11. Subir Kumar, Saha. Introduccin a la Robtica. I Edicin, Mc Graw Hill, 2010.
Pginas Web:
http://www.consult-g2.com/course/chapter9/chapter.html
http://en.wikipedia.org/wiki/Nickel%E2%80%93cadmium_battery
73
14. Motores Sincrnicos http://en.wikipedia.org/wiki/Synchronous_motor
http://elabz.com/brushless-dc-motor-with-arduino/
http://www.cogsci.uci.edu/~noros/android_car.html
http://iteadstudio.com/store/index.php?main_page=product_info&cPath=18&produ
cts_id=307
http://arduino.cc/en/Reference/FunctionDeclaration
http://arduino.cc/en/Reference/SoftwareSerial
http://www.smacula.co.uk/2011/07/arduino-serial-communication.html
http://arduino.cc/en/Reference/Serial
http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
http://www.starlino.com/imu_kalman_arduino.html
http://www.starlino.com/quadcopter_acc_gyro.html
http://es.wikipedia.org/wiki/%C3%81ngulo_de_ataque
http://www.investopedia.com/articles/technical/060401.asp#axzz1xSRDjgzN
http://www.atmel.com/Images/8271S.pdf
http://www.sparkfun.com/datasheets/Components/SMD/Datasheet_IDG500.pdf
http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf
75
APNDICES
En este apartado se debe de incluir informacin que por su naturaleza no puede ser
incluida en el desarrollo mismo del proyecto, pero que fue utilizada para su elaboracin.
void setup() {
Serial.begin(115200); // define el baudrate a 115200 bps
pinMode(arriba, OUTPUT);
pinMode(abajo, OUTPUT);
pinMode(derecha, OUTPUT);
pinMode(izquierda, OUTPUT);
pinMode(centro, OUTPUT);
void loop() {
if (Serial.available() > 0) {
// lee el byte transmitido:
incomingByte = Serial.read();
76
{
digitalWrite(arriba, HIGH);
delay(100);
digitalWrite(arriba, LOW);
delay(100);
}
{
digitalWrite(abajo, HIGH);
delay(100);
digitalWrite(abajo, LOW);
delay(100);
}
{
digitalWrite(derecha, HIGH);
delay(100);
digitalWrite(derecha, LOW);
delay(100);
}
{
digitalWrite(izquierda, HIGH);
delay(100);
digitalWrite(izquierda, LOW);
delay(100);
}
Serial.flush();
}
}
77
Apndice 2. Cdigo completo de programacin del cuadracptero
//Sketch para probar controlador PID con el IMU. Se va a utilizar para variar la frecuencia de muestreo del IMU y ver su
impacto en la senal de salida. Se utiliza solamente un PID por eje y las helices pequenas. Da buen resultado con los
parametros actuales. Todava es inestable.
//Se usa una frecuencia de muestreo de 100 Hz
/*
Hardware:
#include "Ultrasonic.h"
#include "PID_v1.h"
#include <EEPROM.h>
double altura;
int altura2;
Ultrasonic ultrasonic(12,13); // Trigger en Pin 12 y Echo en Pin 13
double time_1_debug;
double time_now_debug;
struct {
char inpInvert[INPUT_COUNT]; // bits 0..5 invert input
int zeroLevel[INPUT_COUNT]; // 0..2 accelerometer zero level (mV) @ 0 G
// 3..5 gyro zero level (mV) @ 0 deg/s
int inpSens[INPUT_COUNT]; // 0..2 acceleromter input sensitivity (mv/g)
// 3..5 gyro input sensitivity (mV/deg/ms)
float wGyro; // gyro weight/smooting factor
78
} config;
// "w" se refiere a cada uno de los ejes, por ejemplo RwAcc[0],RwAcc[1],RwAcc[2] means RxAcc,RyAcc,RzAcc
//Variables tienen que ser globales (se utilizan en getEstimatedInclination)
float RwEst[3]; //Rw estimado de la combinacin de RwAcc y RwGyro
unsigned long lastMicros;
//Variables para uso del controlador PID. Todas tienen que tener formato de double por definicion de la funcion.
//Constantes PID
//Controladores PID. Se dedica 1 controlador a cada eje de rotacin y otro para la altura. Nomenclatura PID(&Input,
&Output, &Setpoint, Kp, Ki, Kd, Direction)
int vel1, vel2, vel3, vel4; //Velocidades individuales finales de los motores
int motor1=3, motor2=9, motor3=10, motor4=11; //Pines de ubicacin de los motores
int Activado=0; //Para inicializar el dispositivo con los motores apagados. Cero Apagados, uno encendidos
int altitud_maxima= 100; //Altitud maxima deseada en centimetros
int general_vel=155; //Velocidad general de los motores
int vel_limit_PID = 20; //Limite de velocidad para mbito de salida de controlador PID postura
int vel_limit_PID_altura = 20; //Limite de velocidad para mbito de salida de controlador PID altitud
double vel_prot = 200; //Limite de proteccin de velocidad para las pruebas.
79
double sensibilidad_control = 0.4; //Sensibilidad del joystick. No usar valores superiores a 0.5!
double vel_altura_manual = 0;
double vel_altura_manual2 = 0;
void setup() {
static int i;
Serial.begin(115200);
//Se asignan los pines de salida para los PWM de los motores
pinMode(motor1, OUTPUT);
pinMode(motor2, OUTPUT);
pinMode(motor3, OUTPUT);
pinMode(motor4, OUTPUT);
//Se inicializan las salidas de los motores para activar los ESCs
analogWrite(motor1,140);
analogWrite(motor2,140);
analogWrite(motor3,140);
analogWrite(motor4,140);
for(i=3;i<=4;i++){
config.inpSens[i] = 2000; // Gyro Sensitivity mV/deg/ms
config.zeroLevel[i] = 1350; // Gyro Zero Level (mV) @ 0 deg/s
}
config.inpInvert[0] = 1; //Acc X
config.inpInvert[1] = 1; //Acc Y
config.inpInvert[2] = 1; //Acc Z
config.wGyro = 10; //Variable para asignar el peso de las lecturas del giroscopio
firstSample = 1;
PID_EJE_X.SetSampleTime(20);
PID_EJE_Y.SetSampleTime(20);
PID_Altura.SetSampleTime(100);
void loop() {
ComunicacionBluetooth();
time_now=millis();
//Mediciones a 100 Hz
if((time_now-time_1)>10) {
time_1=time_now;
getEstimatedInclination();
//Mediciones a 10 Hz
if((time_now-time_2)>100) {
time_2=time_now;
altura2= altura;
altura= ultrasonic.Ranging(CM);
if(abs(altura-altura2)>3) altura=altura2; //Si la diferencia es mayor a 3 cm se pone de nuevo el valor anterior. Esto para
evitar valores no deseados en forma de picos.
PID_EJE_X.Compute();
PID_EJE_Y.Compute();
PID_Altura.Compute();
actualizar_velocidades();
if(vel1>vel_prot||vel2>vel_prot||vel3>vel_prot||vel4>vel_prot){
Activado=0; //apaga motores
81
}
imprimir_valores_debug();
void imprimir_valores_debug(){
time_now_debug=millis();
if((time_now_debug-time_1_debug)>50) {
time_1_debug=time_now_debug;
Serial.print(interval); //microseconds since last sample, please note that printing more data will increase interval
Serial.print(",");
Serial.print((vel_adelante*0.01)); //velocidad PID derecha
Serial.print(",");
Serial.print(x_angle); //Inclination Y axis (estimated / filtered)
Serial.println("");
/*
//Data para debug de la velocidad de los motores
time_now=millis();
if((time_now-time_1)>200) {
time_1=time_now;
Serial.println("Xaxis ");
Serial.println(x_angle, DEC);
Serial.println("Yaxis ");
Serial.println(y_angle, DEC);
Serial.println(" ");
Serial.println("Vel1");
Serial.println(vel1, DEC);
Serial.println("Vel2");
Serial.println(vel2, DEC);
Serial.println("Vel3");
Serial.println(vel3, DEC);
Serial.println("Vel4");
Serial.println(vel4, DEC);
Serial.println("XaxisRaw: ");
Serial.print(analogRead(0));
82
Serial.print(", ");
Serial.print("YaxisRaw: ");
Serial.print(analogRead(1));
Serial.print(", ");
Serial.print("ZaxisRaw: ");
Serial.print(analogRead(2));
Serial.print(", ");
Serial.print("Zero Level: ");
Serial.print(config.zeroLevel[0]);
Serial.println("");
*/
void actualizar_velocidades(){
static int i;
if (Serial.available() > 0)
{
// Lee los 4 bytes del control remoto:
for(i=0;i<4;i++) {
incomingByte1[i] = Serial.read();
// Serial.print("I received: ");
// Serial.println(incomingByte1[i], DEC);
83
}
if (incomingByte1[0]==83) {
altura_sp= (incomingByte1[1]*altitud_maxima)/100;
vel_altura_manual2 =vel_altura_manual;
vel_altura_manual= (incomingByte1[1]*altitud_maxima)/100;
if(vel_altura_manual==255) vel_altura_manual=vel_altura_manual2;
//Comando para los setpoints de inclinacion. Recibe los comandos del joystick de la aplicacion.
if(incomingByte1[0]==43){
if(incomingByte1[1]<=5) adelante_sp=0;
if (incomingByte1[0]==45){
if(incomingByte1[1]<=5) adelante_sp=0;
if (incomingByte1[2]==43){
if(incomingByte1[3]<=5) derecha_sp=0;
if (incomingByte1[2]==45){
if(incomingByte1[3]<=5) derecha_sp=0;
if (incomingByte1[0]==66) { //66 es el valor del primer byte correspondiente a los botones del 1 al 5
//normalize vector (convert to a vector with same direction and with length 1)
normalize3DVector(RwAcc);
if (firstSample){
for(w=0;w<=2;w++) RwEst[w] = RwAcc[w]; //initialize with accelerometer readings
}else{
//evaluate RwGyro vector
if(abs(RwEst[2]) < 0.1){
85
//Rz is too small and because it is used as reference for computing Axz, Ayz it's error fluctuations will amplify leading
to bad results
//in this case skip the gyro data and just use previous estimate
for(w=0;w<=2;w++) RwGyro[w] = RwEst[w];
}else{
//get angles between projection of R on ZX/ZY plane and Z axis, based on last RwEst
for(w=0;w<=1;w++){
tmpf = getInput(3 + w); //get current gyro rate in deg/ms
tmpf *= interval / 1000.0f; //get angle change in deg
Awz[w] = atan2(RwEst[w],RwEst[2]) * 180 / PI; //get angle and convert to degrees
Awz[w] += tmpf; //get updated angle according to gyro movement
}
//estimate sign of RzGyro by looking in what qudrant the angle Axz is,
//RzGyro is pozitive if Axz in range -90 ..90 => cos(Awz) >= 0
signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;
}
firstSample = 0;
}