PROYECTO FIN DE CARRERA
Autor: Honorio Romero Rodríguez
Tutor: Guillermo Heredia Benot
Sevilla, Mayo 2014
2014
UNIVERSIDAD DE SEVILLA
ESCUELA TÉCNICA SUPERIOR DE INGENIERÍA
Ingeniería Industrial Departamento de Ingeniería de Sistemas y Automática
DISEÑO Y CONSTRUCCIÓN DE UN VEHÍCULO AUTÓNOMO DE DOS RUEDAS
Diseño y construcción de un vehículo autónomo de dos ruedas 2
Honorio Romero Rodríguez
Índice de documentos
Parte I ............................................................................................................................................ 7
1.1. Introducción ....................................................................................................................... 8
1.2. Estudios existentes ............................................................................................................. 9
Parte II ......................................................................................................................................... 11
2.1 Características de la bicicleta ............................................................................................ 12
2.2 Bastidor ............................................................................................................................. 14
2.2.1 Modificación para la adaptación del servo de dirección. .......................................... 14
2.2.2 Modificación para la adaptación del motor de tracción. ........................................... 16
2.2.3 Modificación para el soporte de elementos de control ............................................. 17
2.2.4. Fase de pintado ......................................................................................................... 18
2.3. Actuadores. ...................................................................................................................... 19
2.3.1. Caracterización del motor y construcción del servo de potencia. ............................ 19
2.3.2. Caracterización del motor y construcción del sistema de arrastre. ......................... 28
2.4 Sensores. ........................................................................................................................... 33
2.4.1 Arduimu V2(IMU) [7] ................................................................................................. 33
2.4.2. Potenciómetro. ......................................................................................................... 36
2.4.3. Encoder. ........................................................................................................................ 36
2.5. Electrónica de control ...................................................................................................... 39
2.5.1 Tarjeta Arduino Uno [8] ............................................................................................. 40
2.5.2. PCB [9] ....................................................................................................................... 44
2.6. Comunicaciones ............................................................................................................... 46
2.6.1. Comunicación serie ................................................................................................... 46
2.6.1.4 USB [12] ................................................................................................................... 50
2.6.2 Comunicación I2C [14]............................................................................................... 52
2.6.3 Comunicación PWM [15] ............................................................................................ 55
Parte III ........................................................................................................................................ 57
3.1. Software .......................................................................................................................... 58
3.1.1. Descripción del código de Arduimu V2 ..................................................................... 58
3.1.2. Descripción del código de Arduino ........................................................................... 63
3.1.3. Descripción del código para Matlab .......................................................................... 65
Diseño y construcción de un vehículo autónomo de dos ruedas 3
Honorio Romero Rodríguez
Parte IV ........................................................................................................................................ 69
4.1. Estudio económico ........................................................................................................... 70
4.1.1. Viabilidad ................................................................................................................... 70
4.1.2. Rentabilidad .............................................................................................................. 70
4.1.3. Coste de producción.................................................................................................. 72
Parte V ......................................................................................................................................... 73
5.1. Pruebas y experimentos. .................................................................................................. 74
Parte VI ........................................................................................................................................ 85
6.1. Conclusiones..................................................................................................................... 86
6.2. Líneas de desarrollos futuras ........................................................................................... 87
Parte VII ....................................................................................................................................... 88
7.1. Bibliografía ....................................................................................................................... 89
Parte VIII ...................................................................................................................................... 90
8.1. Códigos ............................................................................................................................. 91
Install_arduino .................................................................................................................... 91
Arduino.m ............................................................................................................................ 92
Codigo_Arduino_A3_A4 .................................................................................................... 124
Arduimu ............................................................................................................................. 133
8.2. Datasheets ...................................................................................................................... 151
Transistores puente H, TIP142 y TIP147 ........................................................................... 151
Transistor 2N3055: ............................................................................................................ 152
Transistor BD140 para la etapa intermedia del control de tracción: ................................ 153
Transistores de señal: ........................................................................................................ 154
8.3. Herramientas .................................................................................................................. 156
Soldadora MIG en atmosfera de oxigeno......................................................................... 156
Piedra desbastadora. ......................................................................................................... 157
Materiales para el PCB. ..................................................................................................... 158
8.4. Fotolitos .......................................................................................................................... 159
Fotolito PCB ....................................................................................................................... 159
Diseño y construcción de un vehículo autónomo de dos ruedas 4
Honorio Romero Rodríguez
Resumen del proyecto
La dinámica de una bicicleta es probablemente la dinámica de un móvil más estudiada a la largo de la historia. En 1818 Karl Von Drais[1] demostró que una persona andando en un artilugio con dos ruedas en línea podía mantener el equilibrio girando la rueda delantera. A este artilugio con dos ruedas lo llamo Draisiana. Posteriormente se diseñaron nuevos modelos de bicicleta, hasta el modelo que conocemos hoy pero en el fondo tenían en común que el equilibrio se podía mantener girando la rueda delantera
Figura 1. Draisiana
En 1897 el matemático francés Emmanuel Carvallo[2], y en 1899, Francis Whipple[3],
utilizaron ecuaciones de la dinámica del cuerpo rígido para demostrar teóricamente lo
que era conocido en la práctica, que algunas bicicletas podrían, si se están moviendo
en el rango de velocidades adecuado, equilibrarse solas
Este proyecto forma parte de otro más amplio. Esta parte se va a centrar
particularmente en el diseño y construcción del robot. El hardware necesario para que
en una segunda parte se genere los códigos adecuados para que el robot se mantenga
estable a una velocidad o realice algún tipo de circuito bajo control.
Para ello se ha utilizado una bicicleta real tamaño niño, la cual se ha modificado y
adaptado para transportar una serie de componentes que la hacen autónoma. Este
equipamiento básicamente está compuesto por un portátil, una unidad de medición
inercial, un Arduino uno y un encoder comunicados vía serie y vía i2c que gestionan y
comandan dos motores localizados en la dirección y en la tracción de la bicicleta
mediante un circuito impreso de potencia.
Diseño y construcción de un vehículo autónomo de dos ruedas 5
Honorio Romero Rodríguez
Figura 2. Diagrama de bloques
En la figura anterior puede observarse como Arduino gestiona todas las entradas
enviando vía serie la información a Matlab, este genera las órdenes oportunas y las
envía de nuevo a Arduino que controlará tanto la tracción como la dirección mediante
señales moduladas de ancho de pulso.
Este proyecto comienza con la elección de los motores que serán los responsables del
equilibrio y movilidad de la bicicleta, a continuación se construye un circuito capaz de
controlar independientemente y mediante entradas PWM estos motores.
Posteriormente se elige un chasis adecuado, modificándolo para albergar todos los
componentes. Por último se casa todo y se procede al cableado, habiendo antes
caracterizado y probado cada parte.
Diseño y construcción de un vehículo autónomo de dos ruedas 6
Honorio Romero Rodríguez
Agradecimientos
A mi director de proyecto Guillermo Heredia Benot, por su disponibilidad y entusiasmo a la hora de ayudarme con los distintos problemas que me han ido surgiendo a lo largo del desarrollo del proyecto.
Diseño y construcción de un vehículo autónomo de dos ruedas 7
Honorio Romero Rodríguez
Parte I
Introducción
Diseño y construcción de un vehículo autónomo de dos ruedas 8
Honorio Romero Rodríguez
1.1. Introducción
Desde que el hombre empezó a caminar erguido, ha estado buscando la manera de simplificar su trabajo mediante artilugios. Diseñaba herramientas y utensilios que le ayudaban a desempeñar mejor sus actividades diarias. Con la Revolución Industrial en el s. XIX se construyó maquinaria que facilitase el trabajo a los trabajadores de las fábricas, incrementándose la producción y reduciendo el precio del producto final. Pero lo que de verdad ha supuesto un cambio en la fabricación y en la vida del ser humano en general ha sido la aparición de la robótica, automatizando procesos y facilitando el trabajo en todas las áreas de nuestras vidas. La robótica ha sido de tal importancia que se ha aplicado en casi todos los campos, desde fábricas, lanzamientos de transbordadores espaciales hasta elementos domésticos pasando por aplicaciones militares o de protección ciudadana. La robótica es probablemente una de las partes más importantes y complicadas de la ingeniería, donde confluyen varias disciplinas de la ingeniería para llevar a cabo un proyecto ambicioso. Dentro de la robótica hay que destacar una de sus principales diferenciaciones, que es según su tipo de estabilidad: estática o dinámica. Un robot es de estabilidad estática cuando su funcionamiento no afecta a su centro de gravedad. Poseer estabilidad dinámica no implica poseer estabilidad estática. Para que un robot posea estabilidad estática su centro de gravedad debe estar dentro de su polígono de soporte. El polígono de soporte es la proyección de sus puntos de soporte en la superficie sobre la que se encuentra. En un robot de dos puntos de apoyo el polígono es una línea y por tanto no es estable estáticamente. En este proyecto se ha decidido estudiar un robot de estabilidad dinámica. Esta clase de estabilidad es más complicada desde el punto de vista del diseño y del control, pero ofrece ventajas muy importantes frente a los robots de estabilidad estática. Los robots de estabilidad dinámica tienen la capacidad de adaptarse al terreno que los rodea. Se ha optado para este proyecto por controlar la estabilidad de una bicicleta. La bicicleta presenta un problema conocido como el péndulo invertido. Según varios estudios hechos por varias universidades, una bicicleta sin controlar es capaz de mantenerse estable siempre que su velocidad no disminuya de 5 m/s. Por tanto el proyecto se centra en mantener la bicicleta estable por debajo de esa velocidad.
Diseño y construcción de un vehículo autónomo de dos ruedas 9
Honorio Romero Rodríguez
1.2. Estudios existentes
Como ya se comentó anteriormente, las dinámicas de la bicicleta son probablemente las más estudiadas a lo largo de la historia reciente. Uno de los modelos matemáticos más famosos es el modelo de Francis Whipple. Francis Whipple utilizo las ecuaciones de dinámica del cuerpo rígido para demostrar teóricamente lo que era conocido en la práctica, que algunas bicicletas podrían, si se están moviendo en el rango de velocidades adecuado, equilibrarse solas. La mayoría de los estudios que existen sobre la estabilidad de la bicicleta son estudios teóricos, como el estudio anterior. Cada uno de los estudios que hay normalmente se centra en partes distintas de la estabilidad o tienen enfoques distintos. Éste es el caso por ejemplo de David E. H. Jones[4], quien en 1970 publicó un artículo enPhisics Today, en el cual explicaba que los efectos giroscópicos no son necesarios para equilibrar una bicicleta. Desde 1971, cuando él identificó y dio nombre a los modos de tambaleo, zigzagueo y vuelco. Robin Sharp ha escrito regularmente sobre el comportamiento de motocicletas y bicicletas. En 2007, Meijaard[5], publicó las ecuaciones de movimiento canónicas linearizadas, en el Proceedings of the Royal Society, junto con la verificación por dos métodos distintos. Estas ecuaciones asumen que los neumáticos ruedan sin deslizar, y que el ciclista está sujeto rígidamente al cuadro trasero de la bicicleta. En 2011 el equipo de investigadores de la Universidad Tecnológica de Delft, dirigidos por J.D. G. Kooijman[5], diseñó un nuevo tipo de bicicleta (bautizado como patín de dos masas o PDM), que corrige a sí misma la trayectoria para evitar una caída incluso sin una torsión giroscópica en la rueda delantera y sin los efectos de ángulo de avance, los dos efectos que se creía hasta ahora garantizaban la auto-estabilización de las bicicletas. Este descubrimiento fue publicado en la revista Science.
Figura 3. Patín de dos masas
Diseño y construcción de un vehículo autónomo de dos ruedas 10
Honorio Romero Rodríguez
En estos momentos hay varios estudios abiertos sobre la estabilidad de la bicicleta. La Universidad Tecnológica de Delft (Holanda) lleva investigando sobre la estabilidad de la bicicleta desde 2003. Comenzaron estudiando la estabilidad de una bicicleta sin controlar, y determinaron el rango de velocidades en el cual la bicicleta era estable. Realizaron varios ensayos a distintas velocidades y registraron el comportamiento de la bicicleta en todos los ensayos mediante sensores conectados a un ordenador. Posteriormente han realizado estudios sobre la influencia del pedaleo sobre la estabilidad de la bicicleta. Además en el año 2010 diseñaron un control de estabilidad para la bicicleta. Esta vez en vez de utilizar una bicicleta de verdad utilizaron LEGO para construir un prototipo. En 2005 la compañía Murata Manufacturing Co., Ltd. desarrolló el "Murata Boy"[6], un robot montado en una bicicleta que es capaz de mantener la estabilidad hasta cuando está parado. Esto lo consigue gracias a un volante de inercia dentro del robot, que hace girar en un sentido o en otro dependiendo de la inclinación medida por el giróscopo. Además dispone de un detector ultrasónico para evitar obstáculos, un detector de impactos para detectar las vibraciones debidas baches o terrenos irregulares y otro giróscopo para detectar el giro y poder calcular la posición de la bicicleta a partir de la velocidad lineal y la señal de este giróscopo.
Figura 4. Murata Boy
Diseño y construcción de un vehículo autónomo de dos ruedas 11
Honorio Romero Rodríguez
Parte II
Diseño y fabricación del
vehículo autónomo
Diseño y construcción de un vehículo autónomo de dos ruedas 12
Honorio Romero Rodríguez
2.1 Características de la bicicleta
Como se observa en la figura 5, este proyecto está formado principalmente por el
bastidor original de una bicicleta de niño cuya altura es de 46 centímetros sin contar el
sillín y de 90 centímetros entre las partes más alejadas de las ruedas trasera y
delantera. Su peso original era de los 13.2 kilos. De esta bicicleta original solamente se
ha mantenido su cuadro ambas ruedas, horquilla y sus rodamientos, abrazaderas y la
cadena de arrastre.
Tomando como base este bastidor se diseñaron una serie de soportes para poder
albergar todo el conjunto de componentes que forman el hardware, es decir para la
parte del control de la dirección, se ha utilizado un motor de elevalunas, para la parte
de la tracción se ha utilizado un motor de limpiaparabrisas, que se detallaran más
adelante. Los soportes de ambos componentes se han diseñado y construido en acero
de 2 milímetros de espesor, que posee la resistencia necesaria para no generar
desplazamientos relativos no deseados entre el bastidor y los motores, y al mismo
tiempo tienen la ligereza óptima para no elevar demasiado el peso del conjunto.
El motor de elevalunas forma parte de un sistema algo más complicado, un servo de
potencia en el que se diferencian además de éste, un potenciómetro sensor de control
y un tren de engranajes. De esto último se hablara con más detalle.
Para que el motor de tracción arrastre la cadena, se le ha acoplado un piñón de
bicicleta algo más grande que el piñón de la rueda trasera.
Para la parte de control, formada por el PC, la placa PCB y el Arduino uno igualmente
se ha usado el mismo acero, generando una superficie en la parte trasera del bastidor
suficiente para colocar los componentes. Con el fin de afianzar el PC a dicho soporte
también se han construido tanto una capa de goma antideslizante, como una
abrazadera que evitan el movimiento del portátil sobre la superficie metálica.
Arduino uno va integrado en la placa PCB de control, ya que sobre la baquelita se dejó
espacio a tal efecto. Esta placa PCB como se puede observar es la parte más retrasada
de todo el hardware y está unida a la placa metálica mediante elevadores metálicos
con sus respectivos tornillos a ambos lados.
Otra parte de hardware es el sensor de velocidad, que no es más que un encoder en la
rueda delantera formada por un circuito de barrera infrarroja y led´s de control visual.
Sobre esta barrera infrarroja se desplaza un Cd con acanaladuras en el que alternan
espacios vacíos y material, generando una onda cuadrada de diferente frecuencia, que
como se ha dicho será usada como sensor de velocidad.
Diseño y construcción de un vehículo autónomo de dos ruedas 13
Honorio Romero Rodríguez
La parte que proporciona potencia a todo el sistema es una batería de plomo de 12
voltios y 7 Ah situada en el centro del bastidor y a una altura relativamente baja para
no desequilibrar en exceso al conjunto. Para su fijación se le han construido dos
abrazaderas de chapa fina de hierro que cierran con dos tornillos de 6 mm y otros dos
tornillos de fijación a la chapa que soporta el PC y la PCB.
Inmediatamente por encima de la batería se encuentra la parte más importante en
cuanto a sensores, se trata de una unidad de medición inercial. Que proporciona en
cada instante en ángulo de caída de la bicicleta o también llamado Roll, y el ángulo de
giro del bastidor o Yaw. Esta IMU queda fijada a la batería mediante una placa de
baquelita y dos bridas de plástico.
Todos los componentes se han distribuido de tal manera que mantienen dentro de un
margen aceptable su centro de gravedad inalterado.
Figura5. Robot bicicleta
Diseño y construcción de un vehículo autónomo de dos ruedas 14
Honorio Romero Rodríguez
2.2 Bastidor
Ya se ha comentado en la descripción que como base al proyecto se usa una bicicleta
de niño a la cual se le ha modificado su bastidor. Para poder caracterizar y localizar su
centro de gravedad original y modificado se necesita alguna herramienta de modelado.
Para ello se ha utilizado Solid Edge v19 del cual se han obtenido para la bicicleta
original parámetros físico importantes.
Como se observa en la figura 6, el centro de gravedad está señalado como CV y se
encuentra tomando como centro eje de coordenadas cartesianas en el punto central
del eje del pedalier, en el punto [75.99, 0.0, 102. 64] mm.
El peso del cuadro es de 2,179 kg.
Figura 6. Centro de gravedad bastidor
2.2.1 Modificación para la adaptación del servo de dirección.
De todas las modificaciones sobre el bastidor, esta es quizás la que resulta la más
complicada.
Tomando como base un rectángulo de acero de 2 mm de grosor y dimensiones 28 cm x
6 cm, se van realizando pliegues con una dobladora de planchas metálicas hasta
adaptar su forma de tal manera que abrace al tubo superior del bastidor en su parte
frontal de 3 cm de diámetro. Una vez hecho este pliegue se pasa a construir en esta
misma plancha una base para el motor de servo de la dirección. Esta base debe ser
coplanaria con la superficie superior del tubo donde va alojada la horquilla de la
dirección (figura 7).
Terminada la estructura que soportará al servo se procede a soldarla al bastidor con
soldadura MIG en atmosfera de oxígeno, se ha elegido esta soldadura dado el espesor
pequeño de la chapa y grosor de los tubos que componen la estructura.
Diseño y construcción de un vehículo autónomo de dos ruedas 15
Honorio Romero Rodríguez
La posición del servo será tal que en su conjunto con las demás piezas del hardware de
control, el centro de gravedad quede en el plano central y simétrico del bastidor.
Figura 7. Planos de bastidor
Teniendo todo esto en cuenta, el último paso de esta parte es localizar la posición
adecuada para que el servo pueda mover correctamente el tren de engranajes de la
horquilla. Realizando taladros pasantes en la plancha soldada y en la posición
determinada se obtiene el acoplamiento solidario entre bastidor y el servo de la
dirección, como puede observarse en la siguiente figura.
Figura 8. Acoplamiento motor servo
Terminada la modificación de la estructura se utiliza una amoladora para suavizar
tanto las soldaduras como los bordes cortantes de la semiestructura soldada.
Diseño y construcción de un vehículo autónomo de dos ruedas 16
Honorio Romero Rodríguez
2.2.2 Modificación para la adaptación del motor de tracción.
El punto más importante a tener en cuenta en esta modificación es el hecho de que el
plano del piñón tractor acoplado al motor debe ser coplanario al piñón seguidor de la
rueda trasera y ambos piñones debe estar separados lo suficiente como para que la
cadena de arrastre pueda ajustarse perfectamente sin peligro de salirse y sin estar
demasiado tensa para realizar el movimiento relativo. Al igual que antes se utiliza una
chapa de 2 mm de grosor en forma rectangular de dimensiones 6x5 cm.
A continuación se suelda al bastidor a una distancia previamente seleccionada según la
dimensión de la cadena de arrastre y con la misma soldadora MIG. Aquí no es
necesario ningún pliegue pero necesita algo más de mecanizado para poder ajustarse
al motor. Se realizan tres taladros pasantes para los tres tornillos de anclaje del motor
y un orificio central de 1.5 cm de diámetro para el eje.
Figura 9. Acoplamiento motor de arrastre
En este proceso también se suavizan bordes y soldaduras con una amoladora.
Diseño y construcción de un vehículo autónomo de dos ruedas 17
Honorio Romero Rodríguez
2.2.3 Modificación para el soporte de elementos de control
Tomando una chapa de 2 mm de grosor y dimensiones 43x16 cm se practica en el
centro de su lado pequeño un corte semicircular de diámetro 3 cm. Por otra parte se
toma un rectángulo de caucho antideslizante de 28x16 cm y se recorta un círculo en su
parte delantera de diámetro 3 cm tal que encaje en el tubo del bastidor donde antes
iba colocado el asiento. El soporte quedará solidario al bastidor en tres puntos. Uno de
ellos será una soldadura de contorno semicircular en la parte de la chapa metálica con
dicha forma. Para los otros dos puntos se ha usado una varilla de acero de 6 milímetros
de diámetro y 40 cm de largo doblada en forma de U. El punto tangente de esta U va
soldada al soporte metálico y los dos extremos de la U van soldados al bastidor. En las
siguientes figuras se puede apreciar lo expuesto.
Figura 10. Soporte componentes de control
Para alisar las soldaduras una vez realizadas se usa una amoladora.
Después de haber modificado el bastidor añadiendo soportes para el servo de
potencia, para el motor de tracción y la electrónica de control, esta todo dispuesto
para la imprimación y la pintura.
Diseño y construcción de un vehículo autónomo de dos ruedas 18
Honorio Romero Rodríguez
2.2.4. Fase de pintado
La fase de pintado consta de dos partes. En la primera parte se prepara la superficie
con una lija de grano 600 para que la imprimación quede adherida perfectamente. Una
vez lijado se elimina el polvo con una gamuza atrapapolvo, se limpia con
desengrasante y a continuación se lanza la primera capa de imprimación, llamada de
agarre, con pasadas rápidas de pistola. La presión utilizada es de 2 bares en punta.
Pasados 5 minutos se vuelve a lanzar la segunda y definitiva capa de imprimación,
esperando nueva mente 10 minutos para su secado. La segunda parte de la fase de
pintado es la pintura en sí, que para esta aplicación se ha elegido una monocapa
burdeos que se ha mezclado 2:1 con disolvente acrílico y catalizador. Al igual que para
la imprimación, se han lanzado dos capas de pintura hasta cubrir perfectamente.
Figura 11. Proceso de pintado
Diseño y construcción de un vehículo autónomo de dos ruedas 19
Honorio Romero Rodríguez
2.3. Actuadores.
En este apartado se tratará de caracterizar los motores encargados de girar la horquilla
y dar movimiento de traslación a todo el conjunto. El primero forma parte de un servo
de potencia, que comparte movimientos con un tren de engranajes y un
potenciómetro, necesarios para obtener un giro de horquilla adecuado y una medición
de la posición en todo momento.
2.3.1. Caracterización del motor y construcción del servo de potencia.
El actuador principal de este servo es un motor Bosch 058 brose de 12 Voltios, original
de un Toyota Corolla del 2004, usado como elevalunas trasero derecho el cual se
puede observar en la figura 12.
Figura 12. Motor de elevalunas Al igual que con cualquier motor es importante modelar tanto la parte eléctrica como la mecánica. A continuación se puede observar el modelo eléctrico equivalente del motor.
Figura 13. Modelo eléctrico equivalente motor DC
Diseño y construcción de un vehículo autónomo de dos ruedas 20
Honorio Romero Rodríguez
El modelo mecánico se puede definir como una inercia mecánica, una fricción viscosa y un par resistente que se opone al movimiento del motor. Este modelo se muestra en la figura 14.
Figura 14. Modelo mecánico equivalente motor DC Resumiendo, las variables que se van a obtener son las siguientes: Rm: resistencia interna. Lm: inductancia. Ke: constante eléctrica. Kt: constante mecánica. Jm: inercia mecánica. Dm: fricción viscosa.
Tr: par resistente.
La primera medición que se realiza es la de la resistencia interna (Rm). Esto se realiza
midiendo mediante un multímetro entre los pines de alimentación del motor en vacío.
Esta medición tiene como resultado una resistencia de 1.6 Ω.
Lo siguiente es realizar un ensayo para identificar los parámetros de constante eléctrica (Ke), fricción viscosa (Dm) y par resistente (Tr). El procedimiento para ello es ir variando el factor de servicio desde la máxima velocidad hacia un sentido por ejemplo horario, es decir PWM de 100% e ir disminuyendo éste de 10 % en 10% hasta llega a -100%, máxima velocidad en sentido antihorario. Esto se puede generar mediante órdenes desde Matlab a Arduino que gobierna la placa PCB de potencia. Se verá con más detalle en próximos apartados dedicados a tal efecto. Estando en tal configuración se necesitan tomar tres medidas en cada factor de servicio.
Diseño y construcción de un vehículo autónomo de dos ruedas 21
Honorio Romero Rodríguez
Se han ido anotando los datos de tensión del motor (Vmot), intensidad que circula por el motor (Imot), y velocidad angular (ω) para los factores de servicio estipulados. Apoyándose estas medidas se añade también la fuerza contraelectromotriz fem. La corriente por el motor se mide gracias a que se ha construido un resistencia shunt con una varilla de estaño de 2.5 milímetros de diámetro, destinada a tal efecto y calibrada para que la tensión en milivoltios en dicha resistencia, coincida con la corriente que la atraviesa en Amperios.
Figura 15. Resistencia Shunt
(1)
Factor de servicio (%) Imot(A) Vmot(V) ω (rad/s) fem (V)
100 1.46 10.87 8.97 8.53
90 1.44 10.34 8.49 8.04
80 1.42 9.77 8.05 7.50
70 1.37 9.21 7.57 7.02
60 1.34 8.61 6.98 6.47
50 1.19 7.86 6.34 5.96
40 1.13 6.67 5.37 4.86
30 0.94 4.89 4.05 3.39
20 0.73 2.65 1.92 1.48
10 0.33 0.18 0 0
0 0 0 0 0
-10 -0.41 -0.22 0 0
-20 -0.76 -3.21 -2.48 -1.99
-30 -0.90 -6.01 -4.79 -4.57
-40 -1.12 -7.41 -6.04 -5.62
-50 -1.16 -8.65 -6.83 -6.79
-60 -1.19 -9.31 -7.57 -7.40
-70 -1.21 -9.80 -7.85 -7.86
-80 -1.23 -10.12 -7.95 -8.15
-90 -1.26 -10.38 -8.37 -8.36
-100 -1.34 -10.83 -8.72 -8.69
Tabla 1. Medición motor dirección
Diseño y construcción de un vehículo autónomo de dos ruedas 22
Honorio Romero Rodríguez
En un motor DC cuando se encuentra en rotación, aparece en el inducido una tensión proporcional al producto del flujo por la velocidad angular. Si el flujo es constante como es nuestro caso la tensión inducida fem es directamente proporcional a la velocidad angular.
( ⁄ ) (2)
Figura 16. Constante eléctrica
Finalmente, como se puede apreciar en la figura 16, el valor de Ke queda como 1.0406 Nm/rad y el valor de Kt es el mismo siempre que se utilicen unidades del SI, 1.0406 Nm/A. También se debe calcular la fricción viscosa y el par resistente. Las ecuaciones que utilizan son: (3)
(4)
Para régimen permanente:
(5)
Todos los valores han sido tomados en régimen permanente por lo que es esta ecuación la que se usará para el cálculo de Tr y Dm. Al igual que antes se necesitar un punto por cada factor de servicio de PWM.
Diseño y construcción de un vehículo autónomo de dos ruedas 23
Honorio Romero Rodríguez
Factor de servicio (%) Tm (Nm) ω (rad/s)
100 1.52 8.97
90 1.50 8.49
80 1.48 8.05
70 1.43 7.57
60 1.39 6.98
50 1.24 6.34
40 1.18 5.37
30 0.98 4.05
20 0.76 1.92
0 0 0
-20 -0.79 -2.48
-30 -0.94 -4.79
-40 -1.17 -6.04
-50 -1.21 -6.83
-60 -1.24 -7.57
-70 -1.26 -7.85
-80 -1.28 -7.95
-90 -1.31 -8.37
-100 -1.39 -8.72
Tabla 2. Tabla de medición de Tm
Se han eliminado los factores que no creaban movimiento.es decir el PWM de 10% y
del -10%.
El ajuste se hará por separado para factores de servicio negativos y positivos
Figura17. Gráficos de Dm y Tr
Diseño y construcción de un vehículo autónomo de dos ruedas 24
Honorio Romero Rodríguez
Con estos dos resultados ya se pueden obtener los valores de Dm y Tr haciendo la
media para valores positivos y negativos del par. Dm = 0.1036 Nm • s y Tr = 0.54607
Nm
En el cálculo de la inercia mecánica se va a realizar un ensayo en el que el motor pasará de estar funcionando al 100% de factor de servicio a estar en frenada libre con las conexiones eléctricas flotantes, por lo que sólo actuará la parte mecánica del motor. Para poder visualizar esta parada se ha utilizado la propia placa Arduino y las comunicaciones con la PCB y el Matlab. Este transitorio de parada se registrará en Matlab para poder calcular esta inercia Jm.
Figura 18. Gráfica de ajuste de Jm
La expresión para calcular Jm se toma de (4) partiendo de una velocidad inicial del
experimento al 100% de PWM y teniendo en cuenta que el par aplicado Tm es cero al
dejar las conexiones flotantes, Jm se calcula mediante la siguiente ecuación.
⁄
⁄ (6)
Aplicando los valores y la pendiente calculada de la gráfica de ajuste, Jm=0.0629 Kg m2. Para finalizar este apartado se presenta una tabla con los valores de los parámetros tanto eléctricos como mecánicos del motor Bosch para una mejor visualización de los mismos.
Diseño y construcción de un vehículo autónomo de dos ruedas 25
Honorio Romero Rodríguez
Parámetro Símbolo Valor
Resistencia interna Rm 1.6 Ω
Constante eléctrica Ke 1.0406 Nm/rad
Inductancia Lm 2.06 mH
Constante mecánica Kt 1.0406 Nm/A
Fricción viscosa Dm 0.1036 Nm • s
Inercia mecánica Jm 0.0629 Kg m2
Par resistente Tr 0.54607 Nm
Tabla 3. Resumen de parámetros para el motor Bosch brose
Diseño y construcción de un vehículo autónomo de dos ruedas 26
Honorio Romero Rodríguez
La siguiente parte del servo de potencia viene justo a la salida del eje del motor Bosch y se trata de un tren de engranajes procedentes de una bomba de gasoil de una Iveco Daily 2.2 HPI averiada. El primer paso es sustituir el engranaje de salida original del motor por otro adecuado a las necesidades de giro. En la figura 19 se tiene el engranaje en su forma original y también una vez modificado y acoplado en el motor. Se han eliminado las partes no necesarias y modificado la parte posterior para poder encajarla en el eje del motor. En este proceso se han utilizado limas, una amoladora y nural 23 para evitar cualquier juego entre el eje y el engranaje 1.
Figura 19. Engranaje 1 del tren a la salida del motor A continuación de este engranaje 1 llega el engranaje 2 el cual debe ir en la caña de la horquilla solidario a ella y engranar en éste, el engranaje 1 para poder transmitir el movimiento. Partiendo de las piezas originales de la bomba que se pueden ver en la figura 21.
Figura 21. Despiece engranaje 2
Diseño y construcción de un vehículo autónomo de dos ruedas 27
Honorio Romero Rodríguez
Se corta su eje a la medida necesaria para conectar con la horquilla. El mecanizado de la superficie superior es algo más complejo. Se corta y suaviza la superficie para poder soldar una llave autoclé del número 10, esta llave encajara en el potenciómetro, el cual llevará en su eje una tuerca también del 10. Es decir cuando la horquilla gire girara el engranaje, puesto que la llave autoclé esta soldada, girará también, arrastrando al potenciómetro en su giro. Esta soldadura se ha realizado con una MIG en atmósfera de oxígeno. Alisando la superficie soldada con una piedra desbastadora ya se tiene el engranaje número 2 del tren.
Figura 21. Proceso construcción engranaje 2
Otra parte importante del servo de potencia es el potenciómetro, necesario para conocer en todo momento en que posición se encuentra la dirección. El potenciómetro elegido ha sido un 2K2 giratorio. Al que se le ha practicado una rosca mediante una terraja de la medida 6x100, cortado el sobrante de eje de plástico y acoplado al cuerpo del motor. Ya con todos estos componentes se puede colocar el servo de potencia sobre el soporte del bastidor destinado a ello.
Figura 22. Tren de engranajes y servo acoplado
Diseño y construcción de un vehículo autónomo de dos ruedas 28
Honorio Romero Rodríguez
El último paso es caracterizar la velocidad ideal de salida del eje final del servo, con los datos
del motor, la velocidad máxima de salida del eje motor es de 1.42 Hz. Mediante las relaciones
de engranajes:
(7)
1 max
= 1.42 Hz
N1 = 23 dientes
N2 =37 dientes
ω2max
= 0.88 Hz
La horquilla cuando desde Matlab se genera un factor de servicio de 100% gira a razón de 0.88
Hz como velocidad máxima.
2.3.2. Caracterización del motor y construcción del sistema de arrastre.
El motor elegido para dar movimiento de traslación a la bicicleta es un motor Bosch 058 CEP
de 12 Voltios original de limpiaparabrisas de un Iveco Tector del año 2002. Se muestra en la
siguiente figura.
Figura 23. Motor Bosch para la tracción
Diseño y construcción de un vehículo autónomo de dos ruedas 29
Honorio Romero Rodríguez
El proceso de caracterización es el mismo que para el motor de la dirección por lo que solo se mostraran las gráficas, tablas y resultados significativos. La medición de las resistencia interna resulta Rm= 0.5 Ω. Los parámetros del ensayo al variar el factor de servicio de 100% a 0% se resumen en la tabla siguiente. En este caso por las características del propio motor, es más interesante el estudio de factores de servicio altos, ya que a bajo, el motor no gira.
F. de servicio (%) Imot(A) Vmot(V) ω (rad/s) fem (V)
100 3.45 11.27 6.54 9.54
96 3.26 10.46 6.04 8.83
92 2.95 7.99 4.39 6.51
90 2.92 7.05 3.85 5.59
80 2.84 6.81 3.63 5.39
70 2.52 5.50 2.85 4.24
60 2.23 4.11 2.22 2.99
50 1.82 2.75 1.25 1.84
40 1.40 1.34 0.58 0.64
30 0.95 0.41 0 0
20 - - - 0
10 - - - 0
0 0 0 0 0
Tabla 4. Medición del motor de tracción
Figura 23. Constante eléctrica
Ke=1.4889 Nm/rad y Kt=1.4889 Nm/A.
Diseño y construcción de un vehículo autónomo de dos ruedas 30
Honorio Romero Rodríguez
F de servicio (%) Tm (Nm) ω (rad/s)
100 5.14 6.54
96 4.85 6.04
92 4.39 4.39
90 4.35 3.85
80 4.23 3.63
70 3.75 2.85
60 3.32 2.22
50 2.71 1.25
40 2.08 0.58
30 - -
20 - -
10 - -
0 0 0
Tabla 5. Medición Tm
Figura 24. Gráficos Dm y Tr
Dm=0.4848 Nm/s y Tr=2.1801 Nm.
Diseño y construcción de un vehículo autónomo de dos ruedas 31
Honorio Romero Rodríguez
Figura 25. Gráfica de ajuste de Jm
El pico inicial se debe al ruido producido por el corte del interruptor al dejar las
conexiones flotantes.
Jm=0.1816 Kg m2
Parámetro Símbolo Valor
Resistencia interna Rm 0.5 Ω
Constante eléctrica Ke 1.4889 Nm/rad
Inductancia Lm 0.63 mH
Constante mecánica Kt 1.4889 Nm/A
Fricción viscosa Dm 0.4848 Nm • s
Inercia mecánica Jm 0.1816 Kg m2
Par resistente Tr 2.1801 Nm
Tabla 6. Resumen de parámetros motor tracción
Se muestra también el cuadro resumen del motor del servo para comparar.
Parámetro Símbolo Valor
Resistencia interna Rm 1.6 Ω
Constante eléctrica Ke 1.0406 Nm/rad
Inductancia Lm 2.06 mH
Constante mecánica Kt 1.0406 Nm/A
Fricción viscosa Dm 0.1036 Nm • s
Inercia mecánica Jm 0.0629 Kg m2
Par resistente Tr 0.54607 Nm
Tabla 7. Resumen de parámetros motor de dirección
Diseño y construcción de un vehículo autónomo de dos ruedas 32
Honorio Romero Rodríguez
Caracterizado el motor, se pasa a la parte del sistema de arrastre. Este sistema consta
de la cadena original de la bicicleta, de un piñón añadido al eje del motor y de la rueda
y piñón originales.
Para poder adaptar el piñón al eje del motor se ha soldado concéntricamente una
pieza metálica cilíndrica a la que se le ha practicado un taladro pasante y creado una
rosca de 8x100 mediante machos el juego de machos correspondiente. Para mantener
apretado el piñón al motor, la rosca se ha impregnado de loctite para roscas.
Figura 26. Tren de arrastre
La relación de dientes entre los dos piñones determinará cuál es la velocidad máxima a
la que puede rodar la bicicleta. Siendo el subíndice 1 para el piñón del motor y el 2
para el de la rueda trasera.
(8)
ω1 max
= 6.54 rad/s = 1.04 Hz
N1 = 18 dientes
N2 =16 dientes
ω2max
= 1.17 Hz = 7.51 rad/s
V = W x R (9)
V = 7.51 rad/s x 0.14 m = 1.03 m/s
Diseño y construcción de un vehículo autónomo de dos ruedas 33
Honorio Romero Rodríguez
2.4 Sensores.
Para poder registrar todos los parámetros necesarios para una buena gestión del
control de la estabilidad, se necesitan al menos el ángulo de la horquilla de dirección y
la inclinación del cuadro (Roll), adicionalmente y para posteriores mejoras también se
tendrá otro parámetro, el giro del propio cuadro (Yaw). Para el control de la velocidad
se ha situado un encoder en la rueda delantera. La obtención de todos estos
parámetros se verá con más detalle. En la siguiente figura se muestra en esquema de
las comunicaciones para poder observar claramente cuál es la dirección de la
información.
Figura 27. Diagrama de bloques del conexionado
2.4.1 Arduimu V2(IMU) [7]
Es una Unidad de Medida Inercial, que además posee un procesador compatible con
Arduino, por lo tanto para su programación solamente es necesario abrir la IDE de
Arduino puesto que su código es exactamente igual. Este hardware consta de un
acelerómetro de 3 ejes y tres sensores de giro, regulador de tensión entre 3.3v y 5v,
puerto del GPS, procesador Atmega328 de 16MHz y varios LEDs de estado. Tiene muy
buena relación calidad precio y es fácil de encontrar en el mercado. Para su
programación se ha usado un firmware actualizado y de código abierto al que se le ha
modificado la manera de entregar los datos por el puerto I2C.
Diseño y construcción de un vehículo autónomo de dos ruedas 34
Honorio Romero Rodríguez
Características:
-Diseño plano.
-Bajo costo.
-3 Ejes Acelerómetro.
-3 Eje Giroscopio
-Compatible con Arduino.
-Código fuente incluido y de código abierto.
-Power LED (verde).
-Estado de los LED (rojo, azul, amarillo).
-1 Puerto SPI.
-1 Puerto I2C (será el utilizado en este caso).
-Dos salidas PWM (para servos).
- Puerto GPS.
-Protección de diodo.
-Conector estándar puerto serie.
Al programar Arduimu solo es necesario conectar al PC mediante un cable FTDI y
seleccionar la placa Duemilanove en las propiedades de IDE Arduino.
Figura 28. Arduimu V2
Los parámetros enviados mediante esta IMU, como se comentó anteriormente son el
ángulo de inclinación y el ángulo de giro del cuadro. Estos dos ángulos son enviados en
4 partes, siendo las dos primeras partes, la parte entera y decimal del ángulo de
inclinación o Roll y las dos últimas la parte entera y decimal del ángulo de giro del
cuadro o Yaw. El patrón a seguir en cuanto a los grados enviados es tal que si la
bicicleta está totalmente tumbada hacia la izquierda el Roll será 0 grados mientras
que si esta vertical tendremos 90, para el lado contrario 180 grados.
Diseño y construcción de un vehículo autónomo de dos ruedas 35
Honorio Romero Rodríguez
Es importante comentar que el origen de ángulos siempre será aquel en la que la IMU
sea reiniciada, quiere decir que si todo el sistema se reinicia estando la bicicleta
tumbada, este será el origen de inclinación 90 grados. Por seguir el mismo criterio con
el ángulo Yaw partiendo del estado reiniciado, este será 90 grados, girándolo 90 grados
a la izquierda, este será 0 grados y girando 90 a la derecha desde la posición de
reinicio, se tendrá 180 grados.
La forma en que los datos son enviados desde la IMU a Arduino se verá en apartados
posteriores y se detallará desde el punto de vista del software también en el apartado
correspondiente. Básicamente desde el momento en que enciende y resetea la IMU
pasa tomar origen de ángulos y de forma continua comienza a mandar 4 parámetros,
los cuales corresponden con la parte entera y decimal del Roll y a continuación la parte
entera y decimal del Yaw. Arduino toma estas cuatro cifras y compone los dos ángulos
que serán enviados a Matlab para poder tomar decisiones sobre donde hay que
colocar la nueva posición de la horquilla.
La situación dentro del conjunto del hardware es importante en especial para la IMU
ya que según este orientada así marcara su origen de ángulos. Ésta va situada sobre la
batería en posición horizontal soldada sobre una placa PCB perforada y anclada
mediante bridas a la batería que a su vez esta solidariamente anclada al bastidor con
dos abrazaderas de hierro recubiertas de un asilamiento le plástico blanco y planchitas
de caucho para evitar el deslizamiento.
Figura 29. Situación de Arduimu sobre la batería
Diseño y construcción de un vehículo autónomo de dos ruedas 36
Honorio Romero Rodríguez
2.4.2. Potenciómetro.
El registro de la posición de la horquilla delantera, como se había comentado anteriormente, se hace mediante un potenciómetro de cursor giratorio ya que el movimiento del cursor se asemeja al movimiento que se quiere parametrizar. Se ha seleccionado un resistor variable de manera que la variación de resistencia en función de la posición del cursor sea lineal, así tendremos una ley de variación matemáticamente sencilla. Para cada posición del cursor habrá un valor diferente de resistencia R(α) y con ello de tensión V(R), este último parámetro será el registrado mediante la lectura de las entradas analógicas en Arduino. El potenciómetro seleccionado tiene un valor nominal de 2k2 Ω, si dividimos la tensión de alimentación empleada para dar energía a este sensor entre valor nominal de resistencia, obtendremos en cuanto variará la tensión leída por cada ohm que se incremente debido al desplazamiento del cursor, en otras palabras, la resolución, que indicará cuanto debe variar la posición de la horquilla para notar algún cambio en la lectura del valor indicado por el sensor, si hacemos el cálculo tenemos:
⁄
⁄ ⁄
Para el rango de tensiones en el que se trabaja esta resolución es buena y ante
mínimas variaciones de posición tendremos un cambio apreciable en la lectura del
sensor.
2.4.3. Encoder.
Para poder cuantificar la velocidad de la bicicleta es necesario relacionar ésta con la
variación de alguna magnitud eléctrica, en este caso se ha diseñado un encoder que
registra una onda cuadrada de frecuencia variable y amplitud constante. La frecuencia
de la onda será función de la velocidad de giro de la rueda delantera.
El encoder está formado por una barrera infrarroja atravesada por un disco con
acanaladuras equidistantes. Para comprobar el funcionamiento de forma externa se
han añadido dos led´s. Cuando atraviese la barrera parte sólida del disco se ilumina
uno de los led´s y cuando pase la acanaladura, el led anterior se apaga y se enciende el
otro led. Es decir siempre estará encendido uno de ellos y se alternaran a velocidad
que dependerá del avance de la bicicleta.
Diseño y construcción de un vehículo autónomo de dos ruedas 37
Honorio Romero Rodríguez
El diseño del circuito ha sido realizado al igual que para la placa PCB de potencia, en el
programa Proteus 8. El esquema eléctrico es el siguiente.
Figura 30. Esquema eléctrico del encoder
En su construcción se ha elegido una placa perforada porque el número de
componentes no era muy elevado. A esta placa se le ha soldado un soporte de
baquelita y puesto que se encuentra muy cerca de los radios de la rueda, ha sido
necesario adaptar la forma. El soporte va fijado a la horquilla mediante un tornillo de
6x100. La alimentación del circuito es de 5 Voltios y es tomada de la placa Arduino.
Todo esto puede observarse en las siguientes figuras. El disco con acanaladuras no es
más que un cd limado de forma equidistante en su periferia. Su fijación es solidaria a la
rueda delantera y en este caso se ha pegado con cola térmica
Figura 31. Situación y descripción del Encoder
Diseño y construcción de un vehículo autónomo de dos ruedas 38
Honorio Romero Rodríguez
Conectando el encoder a una entrada analógica de Arduino y este al PC, generando un
código es posible visualizar la onda cuadrada. Es una onda que parte en cero y llega
hasta 3.3 voltios. Es así por el propio diseño del circuito. Extendiendo el experimento
durante 10 segundos y al mismo tiempo variando la velocidad de la rueda se toma la
siguiente gráfica.
Figura 32. Ejemplo de onda de salida del encoder
Como se observa en el ejemplo, empieza con un giro constante y a los cuatro segundos
se para la rueda, para luego girar a más velocidad que antes de los cuatro segundos y
ya a los ocho segundos vuelve a pararse.
La relación entre la frecuencia de giro y la velocidad de la bicicleta se obtiene de la
relación entre varios parámetros, el número de acanaladuras del cd, el radio de la
rueda y el número de pasos por cero de la onda cuadrada en un determinado intervalo
temporal. Con esto último se obtiene la velocidad angular.
El proceso del cálculo de la velocidad angular comienza con la fijación de un tiempo de
muestreo y se relaciona con el número de acanaladuras por ciclo del cd. En cada ciclo
de rueda existen ocho pasos por cero (ocho acanaladuras) en la onda cuadrada, siendo
la circunferencia de la rueda 2πR.
El espacio recorrido por la bicicleta en cada vuelta de rueda es L=0.8796 m. Como
existen ocho acanaladuras, quiere decir que por cada acanaladura se recorre L8=0.1099
m.
Diseño y construcción de un vehículo autónomo de dos ruedas 39
Honorio Romero Rodríguez
La velocidad entonces queda V=S/tmuestreo. El tiempo será elegido según el programa y
la medición del espacio S se hará midiendo los pasos de voltaje de 0 a 3.3 Voltios. El
número de pasos multiplicado por L8 y dividido por el tiempo de muestro dará la
velocidad de la bicicleta.
Por ejemplo en la primera parte de la gráfica del ejemplo de la onda cuadrada, cuando
comienza a girar la rueda entre 1.5 s y 4.5 s, la frecuencia de la onda es constante. Se
recuentan tres saltos en la tensión. Por otra parte el tiempo de muestreo en este caso
hemos tomado 4.5-1.5 s. La velocidad en este tramo elegido sería V=3 x L8/3s=3 x
0.1099/3 m/s=0.1099 m/s
En el segundo tramo de la gráfica, V=3 x 0.1099/2=0.165 m/s.
2.5. Electrónica de control
Este apartado está dedicado a la parte electrónica de control, es decir los
componentes que se encargan del posicionado de la dirección y la velocidad de
traslación de la bicicleta. Toda esta tarea está coordinada mediante Arduino Uno y una
placa PCB que ha sido diseñada y construida para este proyecto.
Puesto que se trata de motores los cuales consumen varios amperios, es necesario que
el diseño soporte la interacción de ambos sistemas (dirección y traslación), al mismo
tiempo.
Mediante una fuente de tensión de 12 voltios y 30 amperios se han realizado una serie
de experimentos sobre varias cargas aplicadas a los ejes de los motores. Para el motor
que compone el servo, aplicando sucesivas cargas se ha colocado un amperímetro
sobre los cables de alimentación llegando a medirse en su estado limite 2.3 amperios,
es decir, en condiciones críticas, el servo consume 27.6 Vatios.
Por otra parte el motor destinado al desplazamiento de la bicicleta tras el mismo
experimento consume una corriente de 3.8 amperios, es decir unos 45.6 Vatios.
Estos datos son interesantes y fundamentales a la hora de diseñar la PCB, puesto que
si se opta por componentes insuficientemente potentes, en cualquier pico de
condiciones, se quemarían.
Diseño y construcción de un vehículo autónomo de dos ruedas 40
Honorio Romero Rodríguez
2.5.1 Tarjeta Arduino Uno [8]
Esta tarjeta es la encargada de recibir la información de los sensores, enviarlas cuando
sean necesarios a Matlab e igualmente gestionar la información recibida por el PC y
llevarla a la PCB, para poder dirigir a la bicicleta a las condiciones requeridas, es decir
trabaja como tarjeta de adquisición de datos y etapa de salida.
Arduino es una plataforma de electrónica abierta para la creación de prototipos
basada en software y hardware flexibles y fáciles de usar. Los programas hechos con
Arduino se dividen en tres partes principales: estructura, valores (variables y
constantes), y funciones. El Lenguaje de programación Arduino se basa en C/C++. En
general, un programa en Arduino tiene la estructura que se observa en la figura 33.
Figura 33. Entorno IDE de programación Arduino
En donde setup() es la parte encargada de recoger la configuración , se invoca una sola vez cuando el programa empieza. Se utiliza para inicializar los modos de trabajo de los pines, o el puerto serie. Debe ser incluida en un programa aunque no haya declaración que ejecutar. La función loop() contiene el código que se ejecutara cíclicamente lo que posibilita que el programa este respondiendo continuamente ante los eventos que se produzcan en
Diseño y construcción de un vehículo autónomo de dos ruedas 41
Honorio Romero Rodríguez
la tarjeta (lectura de entradas, activación de salidas, etc.). Esta función es el núcleo de todos los programas de Arduino y la que realiza la mayor parte del trabajo. Ambas funciones son necesarias para que el programa trabaje. Arduino puede tomar información del entorno a través de sus pines de entrada de toda una gama de sensores y puede incidir sobre aquello que le rodea controlando luces, motores y otros actuadores. Para el desarrollo de esta aplicación en especial se emplearan nueve de las tantas funciones que componen la biblioteca de Arduino, la cuales serán explicadas a continuación: * pinMode(pin, mode) Esta instrucción es utilizada en la parte de configuración setup () y sirve para configurar el modo de trabajo de un PIN pudiendo ser INPUT (entrada) u OUTPUT (salida). pinMode(pin, OUTPUT); // configura “pin” como salida
pinMode(pin, OUTPUT); // configura “pin” como salida.
Los terminales de Arduino, por defecto, están configurados como entradas, por lo tanto no es necesario definirlos en el caso de que vayan a trabajar como entradas.
* Serial.begin(rate) Abre el puerto serie y fija la velocidad en baudios para la transmisión de datos en serie. El valor típico de velocidad para comunicarse con el ordenador es 9600, aunque otras velocidades pueden ser soportadas. *analogRead(pin) Lee el valor de un determinado pin definido como entrada analógica con una resolución de 10 bits. Esta instrucción sólo funciona en los pines (0-5). El rango de valor que podemos leer oscila de 0 a 1023, ejemplo: valor = analogRead(pin); // asigna a valor lo que lee en la entrada “pin” *analogWrite(pin, value ) Esta instrucción sirve para escribir un pseudo-valor analógico utilizando el procedimiento de modulación por ancho de pulso (PWM) a uno de los pines de Arduino marcados como “pin PWM”. El Arduino Uno, que implementa el chip ATmega328, permite habilitar como salidas analógicas tipo PWM los pines 3, 5, 6, 9, 10 y 11. El valor que se puede enviar a estos pines de salida analógica puede darse en forma de variable o constante, pero siempre con un margen de 0-255. analogWrite(pin, valor); // escribe 'valor' en el 'pin' definido como analógico Si enviamos el valor 0 genera una salida de 0 voltios en el pin especificado; un valor de
Diseño y construcción de un vehículo autónomo de dos ruedas 42
Honorio Romero Rodríguez
255 genera una salida de 5 voltios de salida en el pin especificado. Para valores de entre 0 y 255, el pin saca tensiones entre 0 y 5 voltios - el valor HIGH de salida equivale a 5v . Teniendo en cuenta el concepto de señal PWM, por ejemplo, un valor de 64 equivaldrá a mantener 0 voltios de tres cuartas partes del tiempo y 5 voltios a una cuarta parte del tiempo; un valor de 128 equivaldrá a mantener la salida en 0 la mitad del tiempo y 5 voltios la otra mitad del tiempo. Debido a que esta es una función de hardware, en el pin de salida analógica (PWM) se generará una onda constante después de ejecutada la instrucción analogWrite hasta que se llegue a ejecutar otra instrucción analogWrite (o una llamada a digitalRead o digitalWrite en el mismo pin). *Serial.write(value) Escribe datos binarios en el puerto serie. Estos datos se envían como un byte o una serie de bytes. *Serial.read() Lee o captura un byte (un carácter) desde el puerto serie, devuelve -1 si hay ninguno. *Delay(val) Detiene la ejecución del programa la cantidad de tiempo en ms que se indica en la propia instrucción. De tal manera que 1000 equivale a 1seg. *map(value, fromLow, fromHigh, toLow, toHigh) Re-mapea un número desde un rango hacia otro. Esto significa que, un valor (value) con respecto al rango fromLow-fromHight será mapeado al rango toLow-toHigh. No se limitan los valores dentro del rango, ya que los valores fuera de rango son a veces objetivos y útiles. Tener en cuenta que los límites "inferiores" de algún rango pueden ser mayores o menores que el límite "superior" por lo que map() puede utilizarse para revertir una serie de números, por ejemplo: y = map(x, 1, 50, 50, 1); La función maneja correctamente también los números negativos, por ejemplo: y = map(x, 1, 50, 50, -100); también es válido y funciona correctamente. La función map() usa matemática de enteros por lo que no generará fracciones, aunque fuere el resultado correcto. Los resultados en fracciones se truncan, y no son redondeados o promediados.
Diseño y construcción de un vehículo autónomo de dos ruedas 43
Honorio Romero Rodríguez
Parámetros value: el número (valor) a mapear. fromLow: el límite inferior del rango actual del valor. fromHigh: el límite superior del rango actual del valor. toLow: límite inferior del rango deseado. toHigh: límite superior del rango deseado. Devuelve El valor mapeado. A continuación veamos con algo más de detalle en qué consiste una de las técnicas que proporciona Arduino y la cual es empleada para el control de nuestra aplicación, el PWM o modulación por ancho de pulso: La Modulación por Ancho de Pulso (PWM = Pulse Width Modulation) es una técnica para simular una salida analógica con una salida digital. El control digital se usa para crear una onda cuadrada, una señal que conmuta constantemente entre encendido y apagado. Este patrón de encendido-apagado puede simular voltajes entre 0 (siempre apagado) y 5 voltios (siempre encendido) simplemente variando la proporción de tiempo entre encendido y apagado. A la duración del tiempo de encendido (ON) se le llama Ancho de Pulso (pulse Width). Para variar el valor analógico cambiamos, o modulamos, ese ancho de pulso. Si repetimos este patrón de encendido-apagado lo suficientemente rápido por ejemplo con un LED el resultado es como si la señal variara entre 0 y 5 voltios controlando el brillo del LED. En el gráfico de abajo las líneas verdes representan un periodo regular. Esta duración o periodo es la inversa de la frecuencia del PWM. En otras palabras, con la Arduino la frecuencia PWM es bastante próxima a 500Hz lo que equivale a periodos de 2 milisegundos cada uno. La llamada a la función analogWrite() debe ser en la escala desde 0 a 255, siendo 255 el 100% de ciclo (siempre encendido), el valor 127 será el 50% del ciclo (la mitad del tiempo encendido), etc.
Diseño y construcción de un vehículo autónomo de dos ruedas 44
Honorio Romero Rodríguez
2.5.2. PCB [9]
Como se ha comentado en la introducción a este apartado, la placa PCB de potencia recoge dos señales PWM provenientes de Arduino y comanda tanto el motor de la dirección como el de tracción. En el experimento de corriente máxima se han obtenido 2.3 amperios para el motor de la dirección y 3.8 amperios para el de avance. Estos datos servirán para la elección de componentes adecuados para poder desarrollar el trabajo de giro del eje en ambos casos. El circuito de control consta de dos partes , la primera está formada básicamente por
un puente H de 4 transistores de potencia (dos TIP 147 y dos TIP 142)y 4 de
control(2N3904), cuyas entradas llegan de los pines PWM de Arduino Uno. Gracias a
esto el control de la dirección genera a la salida en ambos sentidos y una velocidad de
giro de 0 a 2.3HZ en su eje primario y 0.78 Hz en el eje de la dirección. Observando las
hojas de características de estos transistores de potencia, se tiene que la corriente
máxima por el colector es de 20 Amperios y disipa una potencia máxima de 125 Vatios,
por lo tanto para esta aplicación está sobradamente capacitado.
En cuanto a la parte de tracción, ésta la forman 3 transistores en cascada. La gestión
de la señal de entrada es controlada por el transistor de señal BC547, la etapa
intermedia la forma el BD140 y la salida en potencia es controlada por el transistor
2N3055. Con esta disposición y teniendo una entrada modulada entre 0 y 5 voltios, el
motor de tracción actúa entre velocidades de 0 y 1.04 HZ, lo que se traduce en el eje
de la rueda a velocidades de hasta 1.03 m/s.
En esta parte del circuito, lo más crítico se presenta en la corriente sobre el transistor
2N3055, yendo a su hoja de características, este transistor disipa hasta 115 Vatios, con
una corriente de pico de 15 Amperios.
Todos los transistores están protegidos con sus respectivos diodos.
La herramienta elegida para el diseño y simulación del circuito ha sido Proteus 8,
siendo a la vez intuitiva y muy potente. También ha sido posible la realización del
fotolito para su posterior construcción.
Diseño y construcción de un vehículo autónomo de dos ruedas 45
Honorio Romero Rodríguez
El circuito final se presenta en la siguiente figura.
Figura 34. Esquema eléctrico placa PCB de potencia.
Simulado y testeado positivamente se pasa a su construcción, quedando ya la placa
PCB lista para su montaje y calibrado sobre el bastidor de la bicicleta.
Figura 35. Aspecto final de la placa PCB
Diseño y construcción de un vehículo autónomo de dos ruedas 46
Honorio Romero Rodríguez
2.6. Comunicaciones
En este sistema se pueden diferenciar dos tipos de comunicaciones entre sus
componentes, comunicación serie y comunicación i2c. El resto de interacciones son
mediante variación de tensiones, trenes de pulso y PWM.
En la siguiente figura se observa además de la disposición de los componentes, cual es
la forma en que se comunican entre ellos.
Figura 36. Esquema del conexionado de señales
2.6.1. Comunicación serie
Los componentes que se “hablan” de esta forma son el PC y la placa Arduino Uno,
conectados mediante un cable USB.
El protocolo de comunicación en serie RS-232 es un protocolo estándar utilizado en la
comunicación serie asíncrona. Es el protocolo primario utilizado a través de líneas de
módem.
En la figura 37 se muestra la relación entre los diversos componentes en una
comunicación serie. Estos componentes son la UART, el canal de serie y la lógica de la
interfaz. Un chip de interfaz conocido como el receptor asíncrono o transmisor
universal de UART se usa para implementar la transmisión de datos en serie. La UART
se encuentra entre el equipo host y el canal serie. El canal serie es el conjunto de
Diseño y construcción de un vehículo autónomo de dos ruedas 47
Honorio Romero Rodríguez
cables a lo largo del cual se transmiten los bits. La salida de la UART es un nivel lógico
TTL / CMOS estándar de 0 o 5 voltios.
Figura 37. Comunicación serie
En los protocolos serie asíncronos como RS-232, la estructura consiste en un bit de
inicio, siete u ocho bits de datos, bits de paridad y bits de parada. Un diagrama de
tiempos para una trama RS-232 consiste en un bit de inicio, 7 bits de datos, bits de
paridad uno y dos bits de parada se muestra a continuación en la figura 38. Tenga en
cuenta que la estructura exacta de la trama debe ser acordada por el transmisor y el
receptor antes se debe abrir el COM.
Figura 38. Composición de bits en trama serie
La mayoría de los bits en una trama son fáciles de entender. El bit de inicio se utiliza
para señalar el comienzo de una trama y el bit de parada se utiliza para indicar el final
de una trama. La única parte que probablemente necesite un poco de explicación es el
bit de paridad. La paridad se utiliza para detectar los errores de transmisión. Para la
comprobación de paridad par, el número de 1 de los datos más el bit de paridad debe
ser igual a un número par. Para paridad impar, esta suma debe ser un número impar.
Los bits de paridad se utilizan para detectar errores en los datos transmitidos. Antes de
enviar una trama, el transmisor establece el bit de paridad de manera que el marco
tiene, ya sea par o impar la paridad. El receptor y el transmisor ya han convenido en
qué tipo de paridad (par o impar) se está utilizando. Cuando se recibe la trama,
entonces el receptor comprueba la paridad de la trama recibida. Si la paridad es
incorrecta, entonces el receptor sabe ha producido un error en la transmisión y el
receptor puede solicitar que el transmisor vuelva a enviar la trama.
Diseño y construcción de un vehículo autónomo de dos ruedas 48
Honorio Romero Rodríguez
El tiempo de bit es la unidad básica de tiempo utilizado en la comunicación serie. Es el
tiempo entre cada bit. El transmisor emite un poco, espera el tiempo un poco y luego
emite el siguiente bit. El bit de inicio se utiliza para sincronizar el transmisor y el
receptor. Después de que el receptor detecta la transición de falso-verdadero en el bit
de inicio, se espera un tiempo medio poco y luego comienza a leer la línea serie una
vez cada vez poco después de eso. La velocidad de transmisión es el número total de
bits (información, generales y de inactividad) por el tiempo que se transmiten a través
del enlace serie.
2.6.1.1. Comunicación Arduino Uno-PC
Una vez definidas las características generales de una comunicación serie, se pasa a ver
como lo se comunican detalladamente la placa Arduino y el PC.
En la comunicación con el computador Arduino emplea la comunicación asincrónica.
Esto es, requiere de sólo dos líneas de conexión que corresponden con los pines 2 y 3:
Pin 2 (Rx) pin de recepción y pin 3 (Tx) pin de transmisión, y del establecimiento de un
nivel de tierra común con el computador, esto es, ambas tierras deben estar
conectadas, estableciendo el mismo nivel de voltaje de referencia.
Además de realizar las conexiones físicas entre el microcontrolador y el computador
para que pueda establecerse la comunicación serial debe existir un acuerdo previo en
la manera de cómo van a ser enviados los datos. En este acuerdo se debe incluir los
niveles de voltaje que serán usados, el tamaño y formato de cada uno de los mensajes
(número de bits que constituirán el tamaño de la palabra, existirá o no un bit de inicio
y/o parada, empleara o no un bit de paridad), el tipo de lógica empleada, el orden en
que serán enviados los datos y la velocidad de envío de datos
Arduino facilita este proceso para que sólo sea necesario especificar la velocidad de
envío de los datos. Esta velocidad es conocida como “baud rate” o rata de pulsos por
segundo. Velocidades frecuentes de uso son 9600, 19200, 57600 y 115200.
Diseño y construcción de un vehículo autónomo de dos ruedas 49
Honorio Romero Rodríguez
2.6.1.2 Puerto Serie. [10]
Un puerto serie o puerto serial es una interfaz de comunicaciones de datos digitales,
frecuentemente utilizado por ordenadores y periféricos, donde la información es
transmitida bit a bit enviando un solo bit a la vez, en contraste con el puerto paralelo
que envía varios bits simultáneamente. La comparación entre la transmisión en serie y
en paralelo se puede explicar usando una analogía con las carreteras. Una carretera
tradicional de un sólo carril por sentido sería como la transmisión en serie y una
autovía con varios carriles por sentido sería la transmisión en paralelo, siendo los
vehículos los bits que circulan por el cable.
En tecnologías básicas, un puerto serie es una interfaz física de comunicación en serie
a través de la cual se transfiere información mandando o recibiendo un bit. A lo largo
de la mayor parte de la historia de los ordenadores, la transferencia de datos a través
de los puertos de serie ha sido generalizada. Se ha usado y sigue usándose para
conectar las computadoras a dispositivos como terminales o módems. Los ratones,
teclados, y otros periféricos también se conectaban de esta forma. Mientras que otras
interfaces como Ethernet, FireWire, y USB mandaban datos como un flujo en serie, el
término "puerto serie" normalmente identifica el hardware más o menos conforme al
estándar RS-232, diseñado para interactuar con un módem o con un dispositivo de
comunicación similar.
Actualmente en la mayoría de los periféricos serie, la interfaz USB ha reemplazado al
puerto serie por ser más rápida. La mayor parte de los ordenadores están conectados
a dispositivos externos a través de USB y, a menudo, ni siquiera llegan a tener un
puerto serie.
El puerto serie se elimina para reducir los costes y se considera que es un puerto
heredado y obsoleto. Sin embargo, los puertos serie todavía se encuentran en sistemas
de automatización industrial y algunos productos industriales y de consumo. Los
dispositivos de redes, como los enrutadores y switches, a menudo tienen puertos serie
para modificar su configuración. Los puertos serie se usan frecuentemente en estas
áreas porque son sencillos, baratos y permiten la interoperabilidad entre dispositivos.
La desventaja es que la configuración de las conexiones serie requiere, en la mayoría
de los casos, un conocimiento avanzado por parte del usuario y el uso de comandos
complejos si la implementación no es adecuada.
Diseño y construcción de un vehículo autónomo de dos ruedas 50
Honorio Romero Rodríguez
2.6.1.3 Puerto Asíncrono [11]
A través de este tipo de puerto la comunicación se establece usando un protocolo de
transmisión asíncrono. En este caso, se envía en primer lugar una señal inicial anterior
al primer bit de cada byte, carácter o palabra codificada. Una vez enviado el código
correspondiente, se envía inmediatamente una señal de stop después de cada palabra
codificada. La señal de inicio (start) sirve para preparar al mecanismo de recepción o
receptor, la llegada y registro de un símbolo, mientras que la señal de stop sirve para
predisponer al mecanismo de recepción para que tome un descanso y se prepare para
la recepción del nuevo símbolo. La típica transmisión start-stop es la que se usa en la
transmisión de códigos ASCII a través del puerto RS-232, como la que se establece en
las operaciones con teletipos. El puerto serie RS-232 (también conocido como COM) es
del tipo asincrónico, utiliza cableado simple desde 3 hilos hasta 25 y conecta
computadoras o microcontroladores a todo tipo de periféricos, desde terminales a
impresoras y módems pasando por mouses.
2.6.1.4 USB [12]
El bus universal en serie USB es un estándar industrial que define los cables,
conectores y protocolos usados en un bus para conectar, comunicar y proveer de
alimentación eléctrica entre ordenadores y periféricos y dispositivos electrónicos.
El USB, en este caso servirá para enviar y recibir datos desde y hacia el
microcontrolador, además de proveer al mismo de alimentación. Es un sistema de bus
serie que puede conectarse con varios dispositivos. Hoy en día es muy utilizado para
aplicaciones, tal como la conexión de dispositivos de forma sencilla al ordenador. El
USB ha sustituido al RS-232 porque es más rápido, utiliza baja tensión para el
transporte de datos (3,3V) y es fácil de conectar. Este puerto es Half duplex, lo que
significa que solo puede enviar o recibir datos en un mismo instante de tiempo. El USB
2.0 tiene 4 hilos: VCC, GND, Datos entrada y Datos de salida, con una velocidad de
transferencia de hasta 480 Mbps (60 MB/s) pero por lo general de hasta 125Mbps
(16MB/s). Está presente casi en el 99% de los PC actuales. El cable USB 2.0 dispone de
cuatro líneas, un par para datos, una de corriente y un cuarto que es el negativo o
retorno.
Diseño y construcción de un vehículo autónomo de dos ruedas 51
Honorio Romero Rodríguez
2.6.1.5 Comunicación UART [13]
UART es el Receptor-Transmisor Universal Asíncrono, las funciones principales del chip
UART son las de manejar las interrupciones de los dispositivos conectados al puerto
serie y de convertir los datos en formato paralelo, transmitidos al bus de sistema, a
datos en formato serie, para que puedan ser transmitidos a través de los puertos y
viceversa. El controlador del UART es el componente clave del subsistema de
comunicaciones series de una computadora. El UART toma bytes de datos y transmite
los bits individuales de forma secuencial. En el destino, un segundo UART reensambla
los bits en bytes completos. La transmisión serie de la información digital (bits) a través
de un cable único u otros medios es mucho más efectiva en cuanto a costo que la
transmisión en paralelo a través de múltiples cables. Se utiliza un UART para convertir
la información transmitida entre su forma secuencial y paralela en cada terminal de
enlace. Cada UART contiene un registro de desplazamiento que es el método
fundamental de conversión entre las forma serie y paralelo. El UART normalmente no
genera directamente o recibe las señales externas entre los diferentes módulos del
equipo. Usualmente se usan dispositivos de interfaz separados para convertir las
señales de nivel lógico del UART hacia y desde los niveles de señalización externos. El
chip UART de Arduino recibe o envía señales a nivel TTL por lo que otro chip integrado
en la tarjeta Arduino se encarga de convertir las señales a los niveles de tensión
necesarios para hacer posible la comunicación por el USB y que el ordenador
reconozca al USB como un puerto COM.
Diseño y construcción de un vehículo autónomo de dos ruedas 52
Honorio Romero Rodríguez
2.6.2 Comunicación I2C [14]
El sensor más importante de todo el sistema es la IMU que proporciona la inclinación
lateral y vertical del bastidor, esta IMU envía de forma continua dos ángulos mediante
el protocolo i2c a la placa Arduino Uno, como se puede observar en la figura 36.
I2C es un bus de comunicaciones en serie. Su nombre viene de Inter-Integrated Circuit.
La versión 1.0 data del año 1992 y la versión 2.1 del año 2000, su diseñador es Philips.
La velocidad es de 100 kbit/s en el modo estándar, aunque también permite
velocidades de 3.4 Mbit/s. Es un bus muy usado en la industria, principalmente para
comunicar microcontroladores y sus periféricos en sistemas integrados (Embedded
Systems) y generalizando más para comunicar circuitos integrados entre sí que
normalmente residen en un mismo circuito impreso. La principal característica de I2C
es que utiliza dos líneas para transmitir la información: una para los datos y por otra la
señal de reloj. También es necesaria una tercera línea, pero esta sólo es la referencia
(masa). Como suelen comunicarse circuitos en una misma placa que comparten una
misma masa esta tercera línea no suele ser necesaria.
Las líneas se llaman:
SDA: datos
SCL: reloj
GND: tierra
Las dos primeras líneas son drenador abierto, por lo que necesitan resistencias de pull-up.
Los dispositivos conectados al bus I2C tienen una dirección única para cada uno. También pueden ser maestros o esclavos. El dispositivo maestro inicia la transferencia de datos y además genera la señal de reloj, pero no es necesario que el maestro sea siempre el mismo dispositivo, esta característica se la pueden ir pasando los dispositivos que tengan esa capacidad. Esta característica hace que al bus I2C se le denomine bus multimaestro.
Diseño y construcción de un vehículo autónomo de dos ruedas 53
Honorio Romero Rodríguez
Figura 39. Esquema de comunicación I2C
Las transacciones en el bus I2C tienen este formato:
Figura 40. Formato de datos en comunicación I2C.
El bus esta libre cuando SDA y SCL están en estado lógico alto. En estado bus libre, cualquier dispositivo puede ocupar el bus I2C como maestro. El maestro comienza la comunicación enviando un patrón llamado "start condition". Esto alerta a los dispositivos esclavos, poniéndolos a la espera de una transacción.
El maestro se dirige al dispositivo con el que quiere hablar, enviando un byte que contiene los siete bits (A7-A1) que componen la dirección del dispositivo esclavo con el que se quiere comunicar, y el octavo bit (A0) de menor peso se corresponde con la operación deseada (L/E), lectura=1 (recibir del esclavo) y escritura=0 (enviar al esclavo).
La dirección enviada es comparada por cada esclavo del bus con su propia dirección, si ambas coinciden, el esclavo se considera direccionado como esclavo-transmisor o esclavo-receptor dependiendo del bit R/W. El esclavo responde enviando un bit de ACK que le indica al dispositivo maestro que el esclavo reconoce la solicitud y está en condiciones de comunicarse. Seguidamente comienza el intercambio de información
Diseño y construcción de un vehículo autónomo de dos ruedas 54
Honorio Romero Rodríguez
entre los dispositivos. El maestro envía la dirección del registro interno del dispositivo que se desea leer o escribir. El esclavo responde con otro bit de ACK. Ahora el maestro puede empezar a leer o escribir bytes de datos. Todos los bytes de datos deben constar de 8 bits, el número máximo de bytes que pueden ser enviados en una transmisión no está restringido, siendo el esclavo quien fija esta cantidad de acuerdo a sus características. Cada byte leído/escrito por el maestro debe ser obligatoriamente reconocido por un bit de ACK por el dispositivo maestro/esclavo. Se repiten los 2 pasos anteriores hasta finalizar la comunicación entre maestro y esclavo. Aun cuando el maestro siempre controla el estado de la línea del reloj, un esclavo de baja velocidad o que deba detener la transferencia de datos mientras efectúa otra función, puede forzar la línea SCL a nivel bajo. Esto hace que el maestro entre en un estado de espera, durante el cual, no transmite información esperando a que el esclavo esté listo para continuar la transferencia en el punto donde había sido detenida.
Cuando la comunicación finaliza, el maestro transmite una "stop condition" para dejar libre el bus. Después de la "stop condition", es obligatorio para el bus estar idle durante unos microsegundos.
Diseño y construcción de un vehículo autónomo de dos ruedas 55
Honorio Romero Rodríguez
2.6.2.1. I2C en Arduino/IMU
Para poder disponer de este tipo de comunicación, la placa Arduino utiliza los pines 4 y 5 de las entradas analógicas. Sin más que conectar ambas líneas, ya es posible dicha comunicación. En la parte de programación se podrá ver que parte del código hace esto posible
Figura 41. Pines destinados a I2C en Arduimu V2
2.6.3 Comunicación PWM [15]
Mediante la gestión de la información de los sensores, el sistema genera una salida que indica en qué posición debe estar el manillar y a qué velocidad debe girar el motor de tracción, estas salidas son generadas por Arduino como señales PWM que se mandan a la placa de control de potencia PCB y a continuación a los motores.
La modulación por ancho de pulsos (también conocida como PWM [pronunciado pe dobleuve eme], siglas en inglés de pulse-width modulation) de una señal o fuente de energía es una técnica en la que se modifica el ciclo de trabajo de una señal periódica (una senoidal o una cuadrada, por ejemplo), ya sea para transmitir información a través de un canal de comunicaciones o para controlar la cantidad de energía que se envía a una carga.
El ciclo de trabajo de una señal periódica es el ancho relativo de su parte positiva en relación con el período. Expresado matemáticamente:
Diseño y construcción de un vehículo autónomo de dos ruedas 56
Honorio Romero Rodríguez
D es el ciclo de trabajo
es el tiempo en que la función es positiva (ancho del pulso) T es el período de la función
La construcción típica de un circuito PWM se lleva a cabo mediante un comparador con dos entradas y una salida. Una de las entradas se conecta a un oscilador de onda dientes de sierra, mientras que la otra queda disponible para la señal moduladora. En la salida la frecuencia es generalmente igual a la de la señal dientes de sierra y el ciclo de trabajo está en función de la portadora.
La principal desventaja que presentan los circuitos PWM es la posibilidad de que haya interferencias generadas por radiofrecuencia. Éstas pueden minimizarse ubicando el controlador cerca de la carga y realizando un filtrado de la fuente de alimentación.
La modulación por ancho de pulsos es una técnica utilizada para regular la velocidad de giro de los motores eléctricos de inducción o asíncronos. Mantiene el par motor constante y no supone un desaprovechamiento de la energía eléctrica. Se utiliza tanto en corriente continua como en alterna, como su nombre lo indica, al controlar: un momento alto (encendido o alimentado) y un momento bajo (apagado o desconectado), controlado normalmente por relés (baja frecuencia) o MOSFET o tiristores (alta frecuencia).
Otros sistemas para regular la velocidad modifican la tensión eléctrica, con lo que disminuye el par motor; o interponen una resistencia eléctrica, con lo que se pierde energía en forma de calor en esta resistencia.
Otra forma de regular el giro del motor es variando el tiempo entre pulsos de duración constante, lo que se llama modulación por frecuencia de pulsos. En los motores de corriente alterna también se puede utilizar la variación de frecuencia.
Figura 42. Gestión PWM en Arduino
Diseño y construcción de un vehículo autónomo de dos ruedas 57
Honorio Romero Rodríguez
Parte III
Software
Diseño y construcción de un vehículo autónomo de dos ruedas 58
Honorio Romero Rodríguez
3.1. Software
El objetivo de la programación del hardware que se ha construido, es poner en manos
del programador en entorno Matlab toda la información de los sensores en tiempo
real para que en función de cuál sea el objetivo de control, poder tomar decisiones y
programar éstas. Aparentemente el código más lógico será aquel que intente
mantener en equilibrio a la bicicleta en su avance pero existen muchas más
posibilidades.
Este apartado describirá cómo funciona el programa de la unidad de medición inercial,
el programa instalado en la memoria de Arduino y también el código necesario para
visualizar todos los parámetros en Matlab.
3.1.1. Descripción del código de Arduimu V2
Lo primero que se necesita para poder comunicar el PC con Arduimu es un cable FTDI e
instalar los drivers oportunos. Este cable crea un puerto serie a partir del USB.
El extremo del FTDI posee una clavija de seis pines hembra cuyos cables de señal están
ordenador de la misma forma que el conector macho que posee Arduimu.
Figura 43. Conector FTDI en Arduimu
Tras estas operaciones ya se está en disposición de acceder al firmware y poder
modificarlo según las necesidades. El entorno de programación es el mismo que para
Arduino pero con una serie de características. El firmware de esta IMU es abierto y se
encuentra en internet al igual que la IDE de Arduino y su programación se está basado
en C/C++. La versión del firmware es anterior a 2011 por lo que sólo puede ser abierto
sin errores por versiones de IDE Arduino anteriores a la 0023 seleccionando como
Diseño y construcción de un vehículo autónomo de dos ruedas 59
Honorio Romero Rodríguez
placa Duemilanove ATmega 328 y el puerto COM correspondiente al puerto serie
virtual creado por el cable FTDI.
Figura 44. Entorno IDE Arduino 0023
En su estructura original el código del firmware supone una comunicación vía serie
pero no I2C, por lo que ha de modificarse para poder enviar tanto el Roll como el Yaw
del bastidor vía I2C. Al abrir el firmware se observa que existe un código principal
seguido de varias funciones. La función OUTPUT señalada en la figura siguiente
muestra de que manera Arduimu envía los datos vía serie. Existirían dos posibilidades,
una de ellas sería modificar esta función para el envío I2C pero se perdería la
capacidad de probar la aplicación Ardupilot directamente desde el FTDI. La otra opción
que y por la que se ha optado es modificar la pestaña principal nombrada como
Arduimu.
Estando en la pestaña principal del código, se ve en la imagen resaltada en amarillo la
función WIRE, que se ha añadido al bloque principal para poder hacer uso de la
comunicación I2C en cualquier parte del código. Se observa también que se declara
fuera de cualquier bucle void setup() y justo al principio.
Diseño y construcción de un vehículo autónomo de dos ruedas 60
Honorio Romero Rodríguez
Figura 45. Librería WIRE para comunicación I2C
El código original tiene activadas muchas opciones de la placa IMU flat que no son
necesarias en esta aplicación por lo tanto, para una mejor gestión del micro interno se
han desconcertado y solo se han dejado activa la función generadora de ángulos Roll
Yaw y Pitch, éste último muy usado en aeromodelismo pero que dado que la bicicleta
siempre estará en posición horizontal sobre el suelo, no es útil.
Diseño y construcción de un vehículo autónomo de dos ruedas 61
Honorio Romero Rodríguez
También se ha ajustado la declinación magnética para Sevilla que es de -1.5.
Figura 46. Desactivación de varias funciones no usadas
Ahora ya se está en disposición de modificar y añadir el código para la aplicación de la
bicicleta.
Se crean cuatro variables que inicializamos a cero, estas variables almacenarán las
partes enteras y decimales de los ángulos Roll y Yaw intermedias y también un vector
de cuatro componentes que será lo que envíe.
Figura 47. Variables que almacenan los ángulos
Diseño y construcción de un vehículo autónomo de dos ruedas 62
Honorio Romero Rodríguez
A continuación se declara y arranca la función WIRE, ahora es posible ya que se
inicializo la librería anteriormente (figura 45). Se deja la comunicación serie abierta por
si en algún momento se desea abrir ardupilot en el PC a través del cable FTDI para
corroborar que todos los ángulos miden correctamente (se optó por no modificar la
pestaña OUTPUT).
Figura 48. Declaración de WIRE y su interrupción a I2C si se requiere por el maestro
La tercera fila inicializa a la placa Arduimu como esclavo número cuatro y la cuarta
línea indica que cuando el maestro llame al esclavo número cuatro, éste salte a la
función I2C. Esta modificación se hace dentro del bucle void setup(), es decir es válido
para todas las funciones llamadas luego dentro del bucle loop().
Figura 49. Construcción de los ángulos y envío al maestro
Producida la llamada a I2C desde el maestro, se toman los ángulos y se les suman 90º
para indicar que el origen de la bicicleta es la posición vertical e igualmente 90º es la
Diseño y construcción de un vehículo autónomo de dos ruedas 63
Honorio Romero Rodríguez
posición inicial del bastidor. Luego se guardan las partes enteras y decimales del Roll y
el Yaw en el vector inclinación.
Roll = XX.XX= inclinación[0].inclinacion[1]
Yaw =YY.YY = inclinación[2].inclinacion[3]
Almacenadas los cuatro números de dos cifras cada uno, la última línea de la función
los envía a Arduino, que es el maestro vía I2C.
3.1.2. Descripción del código de Arduino
La placa Arduino Uno trabaja como una tarjeta de adquisición de datos. Como se ha
explicado en la parte referente a la electrónica de control se tienen seis entradas
analógicas que van desde A0 hasta A5, estando las dos últimas reservadas para el bus
I2C. Las dos primeras también se reservan para el potenciómetro y el encoder. En este
apartado interesan las entradas A2 y A3. Es ahí donde virtualmente se modificará el
programa para colocar los valores de los dos ángulos enviados por la IMU.
La programación de la placa es más simple que en el caso anterior, solo es necesario
un cable USB y cualquier versión IDE de Arduino. Al igual que antes se elige el tipo de
placa y el puerto COM correspondiente.
Figura 50. Entorno IDE 1.0.5 para Arduino
Diseño y construcción de un vehículo autónomo de dos ruedas 64
Honorio Romero Rodríguez
Matlab posee un paquete que gestiona la conexión entre Arduino y el propio entorno
Matlab. Se parte de éste para modificar y añadir código.
Figura 51. Declaración de variables y de WIRE en Arduino
Arduino es el maestro en la comunicación I2C por lo tanto también necesita tener
incluida la librería WIRE y así se hace al principio del código. Se crean variables para los
ángulos y también un vector, todo a cero.
Dentro del bucle inicial void setup(), se inicializa comunicación serie, que será usada
por Matlab, se pone a máxima velocidad 115200 baudios y también la I2C con todos
los esclavos.
Figura 52. Apertura de la función e inicialización de WIRE
El comienzo del bucle de repetición void loop(), se inicia con el bloque que le pide al
esclavo número cuatro (IMU), cuatro bytes que ira almacenando en el vector angulo[j],
declarado antes.
Figura 53. Almacenamiento de los ángulos en la memoria de Arduino
Cuando se obtienen los cuatro bytes, se finaliza la comunicación.
Diseño y construcción de un vehículo autónomo de dos ruedas 65
Honorio Romero Rodríguez
Ahora cuando desde Matlab se requiera información de los puertos A2 y A3, el código
llegara al CASE30 en el cual se harán lecturas de los puertos donde se han almacenado
el valor de los ángulos.
Figura 54. Requerimiento desde Matlab de A2 y A3
Las entradas analógicas adquieren valores de 0 a 1023 y no trata con números
decimales, para evitar conflictos con los tipos de variables, los dos ángulos se enviaran
multiplicados por cien, esto no es ningún problema puesto que cuando se tengan en
alguna variable de Matlab, basta con dividir por cien para obtener el ángulo con
decimales. Es decir por último se toman dos primeros componentes del vector ángulo
multiplicando a la parte entera por cien y sumándolo a la parte decimal, esto se mete
en la variables Roll, de igual manera se hace para el Yaw. Con esto en las entradas
analógicas ya se tiene el ángulo de inclinación multiplicado por cien en A3 y el ángulo
del bastidor multiplicado por cien en A2.
3.1.3. Descripción del código para Matlab
El código para Matlab se usa tal cual está en el paquete original, así que sólo hay que
detallar las órdenes que se usan para este proyecto. Para poder hacer uso de Arduino
como tarjeta de adquisición de datos, una vez abierto Matlab y tener la bicicleta
alimentada por su batería.
Estando en el entorno de Matlab el usuario tiene la posibilidad de visualizar cualquiera
de los cuatro parámetros de control. Para ello se debe tener tanto el script
install_arduino como el arduino.m en la carpeta de trabajo, ya sea bin u otra.
Diseño y construcción de un vehículo autónomo de dos ruedas 66
Honorio Romero Rodríguez
Se arranca la función install_arduino.
Figura 55. Install_arduino
A continuación se designa a la placa Arduino que estamos usando con algún nombre y
se abre el puerto serie por su COM correspondiente. En todos los programas se ha
usado cIO (comunicación entrada salida). Si todo está correctamente configurado
aparecerá el siguiente mensaje.
Figura 56. Instalación correcta de la placa Arduino del puerto COM12
Diseño y construcción de un vehículo autónomo de dos ruedas 67
Honorio Romero Rodríguez
Los parámetros inclinación, ángulo de bastidor, velocidad de avance y ángulo de la
horquilla ya están disponibles desde Matlab. Para llamar a estos parámetros se
necesitan una serie de instrucciones que se detallan.
El formato en el que llegan el ángulo Roll y Yaw ya ha sido explicado, en el apartado del
encoder también se explicó que tipo de onda llega a la entrada analógica, pero
teniendo en cuenta que estas entradas van desde 0 a 1023 y que la onda cuadrada del
encoder va desde 0 a 3.3 Voltios, se necesita un factor de conversión. Sin este factor la
onda cuadrada tiene 0 que corresponde a al 0 de voltios en el caso mínimo y sabiendo
que el 1023 corresponde a una entrada máxima de 5 voltios, quiere decir que la onda
que llega del encoder tendrá un valor máximo de 675(3.3 Voltios) en Matlab.
El potenciómetro de la dirección tiene valores comprendidos entre 0 y 5 Voltios, que
se traduce en 0 y 1023 para la entrada analógica. Teóricamente el ángulo en el que la
rueda va alineada al bastidor seria 2.5 (511 en A0) Voltios pero esto se ve cuando este
todo montado y se calibre en su posición inicial la horquilla.
Las entradas analógicas no hay que inicializarlas en Matlab, cosa que no ocurre así en
las digitales, siendo preciso inicializarlas como entradas o como salidas, esto se verá a
continuación.
El comando principal para hacer lecturas de las entradas analógicas es.
Figura 57. Comando de lectura de puertos analógicos
Con esta orden se lee del puerto COM 12 al que está asociado cIO y dentro de esta
tarjeta Arduino se lee la entrada analógica A3.
Por otro lado cuando ya se tiene información de todos los parámetros, se tomarán
decisiones sobre a qué velocidad ir en el siguiente paso de integración y a que ángulo
llevar la dirección, para comandar los dos motores primero hay que ver que pines
digitales se va a usar en la placa Arduino como salidas. Este control es del tipo PWM y
sólo es posible utilizar algunos puertos digitales de esta manera. Los motores
necesitan tres órdenes, una para el motor de avance y dos para el motor de la
dirección. Se han elegido los puertos digitales 3 5 y 6, siendo el 3 y 6 para girar la
dirección a derecha e izquierda respectivamente y el 5 para avanzar. En la
configuración de estos pines como salida PWM se usa la siguiente instrucción.
Diseño y construcción de un vehículo autónomo de dos ruedas 68
Honorio Romero Rodríguez
Figura 58. Configuración pin digital como salida
En este ejemplo se ha configurado el pin 3 de digital como salida PWM.
Las salidas digitales en Arduino van desde el 0 al 255, que equivale a 0 y 5 Voltios. En
otras palabras se está variando el factor de servicio entregado a los motores.
Figura 59. Factor de servicio 100% en pin 3
Con esta última orden Matlab manda a Arduino que en su salida número 3 de pines
digitales configurada como PWM tenga un factor de servicio del 100%, A su vez la
tarjeta Arduino manda a la placa de potencia PCB que gire a toda velocidad, unos 8.97
rad/s, hacia la derecha.
Diseño y construcción de un vehículo autónomo de dos ruedas 69
Honorio Romero Rodríguez
Parte IV
Estudio económico
Diseño y construcción de un vehículo autónomo de dos ruedas 70
Honorio Romero Rodríguez
4.1. Estudio económico
La aplicación principal de este modelo es la docencia. Las características principales de
la bicicleta son la inestabilidad y la no linealidad, bases perfectas para la
implementación de controles complejos.
4.1.1. Viabilidad
El área de la regulación automática, implementación y diseño de controles, es una de
las más extensas dentro de la ingeniería y en la que confluyen otras muchas disciplinas
como la mecánica, transmisión de calor, hidráulica, etc.
La idea principal de este proyecto es la creación de un modelo real sobre el cual
diseñar mecanismos de control y al mismo tiempo monitorizar parámetros
importantes en cada experimento, para así mejorar y verificar cada estrategia de
control aplicada.
En cuanto al diseño exterior de ha optado por una bicicleta real, tamaño pequeño,
siendo el chasis de acero y sobre el cual también se han insertado plataformas y
adaptadores en acero.
4.1.2. Rentabilidad
Este proyecto está dirigido principalmente a departamentos de universidades, sobre
los cuales, los alumnos puedan diseñar sus propios controladores y probarlos en
campo.
La fabricación del prototipo inicial ha requerido piezas muy variadas con precios
también muy variados. A continuación se describen los precios de estos componentes,
consultados muchos en proveedores, ya que se disponía previamente de algunos de
ellos.
Diseño y construcción de un vehículo autónomo de dos ruedas 71
Honorio Romero Rodríguez
Concepto Precio
Bicicleta Chapa acero 0.5x1x2
Mecanismo elevalunas Bosch Motor limpiaparabrisas Bosch
Bomba gasoil de desguace Arduino Uno
Componentes electrónicos PCB
Batería 7 Ah Arduimu V2
Componentes encoder Pintura burdeos
Cableado
86.60 22.10
145.30 76.60 45.30 28.50
32.15 22.10 74.90
2.5 4 8
Total 548.05
Tabla 8. Rentabilidad
Diseño y construcción de un vehículo autónomo de dos ruedas 72
Honorio Romero Rodríguez
4.1.3. Coste de producción
Tabla 9. Coste de producción
Coste por hora de trabajo 40 euros/hora.
Sumando el coste de producción al coste de materiales, el presupuesto para la
construcción del prototipo asciende a 2648.05 euros.
Concepto Numero horas Precio
Desmontar bicicleta original
Modificación del cuadro para
adaptar los componentes
Diseño y construcción PCB´s
Montaje de elementos, cableado y ajustes
1.5
15
30
6
60
600
1200
240
total 2100
Diseño y construcción de un vehículo autónomo de dos ruedas 73
Honorio Romero Rodríguez
Parte V
Experimentos
Diseño y construcción de un vehículo autónomo de dos ruedas 74
Honorio Romero Rodríguez
5.1. Pruebas y experimentos.
Una vez montado todo el robot es interesante saber cómo responde su subsistema
más importante, la dirección. Como se comporta la horquilla en un giro es crítico para
poder llegar a mantener en equilibrio a toda la bicicleta. Por eso se realiza un
experimento en el que se irá variando el factor de servicio y se capturará en gráficas
como es la evolución de la tensión en el servo. Igualmente relacionando el giro del
potenciómetro con su variación de tensión se podrá obtener a qué velocidad gira la
horquilla. Se comenzará inicialmente con un factor de servicio que cree movimiento,
este será su factor PWM crítico por debajo del cual el par resistente del conjunto
motor, tren de engranajes y rodamientos de horquilla es mayor que el par que
produce el motor del servo.
Desde Matlab primero de inicializa las salidas correspondientes al giro a derecha e
izquierda como salidas.
A continuación se partirá de 0 para la salida y se irá aumentando hasta que la horquilla
comience a girar. En ambos sentidos no tiene por qué ser el mismo, el motor por
construcción presentaba desigualdades a la hora de su caracterización en la parte II.
X representa el valor del factor de servicio que en Arduino está entre 0 y 255.
El pin 6 en el robot es el giro a izquierda y el 3 a derecha.
PWMdc critico a derecha resulta ser 70, un 27.45% factor de servicio.
PWMic critico a izquierda es 80, un 31.37% factor de servicio.
PWMdc 27.45%
PWMic 31.37%
Tabla 10. Factores críticos
Conocido cual es el comienzo del movimiento para los dos sentidos se realizaran 5
experimentos para cada sentido en el que se registrará la evolución de la tensión en el
potenciómetro y con ello se calculará la velocidad de salida el servo en el giro de la
horquilla.
Diseño y construcción de un vehículo autónomo de dos ruedas 75
Honorio Romero Rodríguez
La relación que existe entre la variación en tensión del potenciómetro y variaciones de
ángulo se puede determinar mediante otro pequeño experimento que se muestra a
continuación.
Figura 61. Angulo total girado por el potenciómetro
Tomando una tabla y haciéndole un taladro en un punto para poder introducir el
potenciómetro, se dibuja un gráfico circular de radio 39mm partiendo del centro del
potenciómetro para poder conocer cuál es el ángulo girado por éste y por lo tanto
relacionar ambas magnitudes, tensión y ángulo.
Se observa que el potenciómetro gira tres cuadrantes y 30º, es decir 300º. Asumiendo
evolución linear de la resistividad ante el giro, ya se está en condiciones de obtener la
relación buscada µ.
5 Voltios ------- 300 º ----- 5π/3 radianes
(5π/3) /5 rad/V
µ = π/3 rad/V
Diseño y construcción de un vehículo autónomo de dos ruedas 76
Honorio Romero Rodríguez
Se presentan a continuación las evoluciones gráficas mencionadas, siendo a derecha el
sentido para los primeros cinco experimentos.
Figura 62. Velocidad de salida a 27.45%
Figura 63. Velocidad de salida a 41.96%
Diseño y construcción de un vehículo autónomo de dos ruedas 77
Honorio Romero Rodríguez
Figura 64. Velocidad de salida a 56.47%
Figura 65. Velocidad de salida a 70.98%
Diseño y construcción de un vehículo autónomo de dos ruedas 78
Honorio Romero Rodríguez
Figura 66. Velocidad de salida a 100%
% PWM Pendiente V/s w rad/s
27.45 1.48 1.48xµ =1.55
41.96 2.20 2.30
56.47 3.20 3.35
70.98 3.97 4.15
100 4.74 4.97
Tabla 11. Resumen de resultados a derecha
Al final del apartado 2.3.1 idealmente se calculaba que la salida del tren de engranajes,
sería de 0.88 Hz. Realmente no se obtiene esa velocidad puesto que el peso de la
bicicleta y las pérdidas entre elementos con movimiento relativo hace que esta cifra
baje a 4.97 radianes/s, WMAX REAL =0.79 Hz.
Diseño y construcción de un vehículo autónomo de dos ruedas 79
Honorio Romero Rodríguez
A continuación se realiza la misma operación pero con el giro a izquierda.
Figura 67. Velocidad de salida a 31.37%
Figura 68. Velocidad de salida a 45.09%
Diseño y construcción de un vehículo autónomo de dos ruedas 80
Honorio Romero Rodríguez
Figura 69. Velocidad de salida a 58.81%
Figura 70. Velocidad de salida a 72.53%
Diseño y construcción de un vehículo autónomo de dos ruedas 81
Honorio Romero Rodríguez
Figura 71. Velocidad de salida a 100%
% PWM Pendiente V/s w rad/s
31.37 0.59 0.62
45.09 1.70 1.78
58.81 2.59 2.71
72.53 3.68 3.85
100 4.58 4.80
Tabla 12. Resultados a izquierda
Se observa que a izquierda a partir del 72.53 la velocidad es más baja que a derecha
en los mismos factores de servicio y de igual forma la velocidad máxima tampoco
alcanza el máximo teórico, y además es más baja que a derecha máxima.
Diseño y construcción de un vehículo autónomo de dos ruedas 82
Honorio Romero Rodríguez
Por último para reflejar gráficamente esta diferencia, se adjuntan los siguientes
gráficos.
Figura 72. Rectas de regresión sobre ambos sentidos
Figura 73. Variación de velocidades
A bajos factores de servicio el servo gira más rápido a la derecha que a la izquierda,
esta diferencia va disminuyendo conforme aumenta el % PWM, haciéndose
prácticamente igual a un 100%. Este resultado es lógico en cuanto el motor fue
diseñado para trabajar a máxima velocidad como elevalunas.
Diseño y construcción de un vehículo autónomo de dos ruedas 83
Honorio Romero Rodríguez
Existe un rango de velocidades en el que pueden girar de igual forma y éste es
realmente el que interesa a la hora de diseñar como debe de funcionar el servo. Esta
franja se extiende trazando una horizontal desde la mínima velocidad en sentido
derecha hasta otra horizontal trazada desde el máximo de velocidad en el sentido a
izquierda. Existe otra limitación en cuanto al movimiento. El servo sólo comienza a
funcionar para cada sentido a partir de un cierto factor de servicio, así que la franja
también se ve limitada con una vertical en la restricción para el inicio del movimiento,
en este caso %PWM del 31.37 perteneciente al giro a izquierda. Con la última
restricción el mínimo de la franja horizontal también debe aumentar. Gráficamente el
resultado es mucho más claro.
Figura 74. Franja útil
A la hora de programar, la horquilla girará entre 1.77 rad/s y 4.80 rad/s. En la imagen
se observa un máximo de 5.11, esto se debe a la recta de regresión ajusta los puntos y
distorsiona un poco el resultado, pero el máximo real es 4.80 rad/s. Para poder saber
que factor de servicio hay que usar para que giren a igual velocidad tan sólo hay que
trazar un horizontal por la velocidad deseada y bajar en el punto de corte hasta el
factor de servicio para ambos sentidos.
Diseño y construcción de un vehículo autónomo de dos ruedas 84
Honorio Romero Rodríguez
Por ejemplo si cuando el programador decide que para mantener en equilibrio a la
bicicleta el giro debe de ser en un momento concreto 3 rad/s, se procedería de la
siguiente manera.
Las rectas obtenidas fueron:
Sentido derecha
Sentido izquierda
Imponiendo 3 rad/s en ambas rectas.
PWMd = 53.93 %
PWMi = 65.62 %
Figura 75. Ejemplo de cálculo de velocidades
Las órdenes para estos giros en igualdad de velocidad serian deshaciendo el cambio
de %PWM a unidades reconocidas por Arduino de 0 a 255.
Diseño y construcción de un vehículo autónomo de dos ruedas 85
Honorio Romero Rodríguez
Parte VI
Conclusiones y desarrollos
futuros
Diseño y construcción de un vehículo autónomo de dos ruedas 86
Honorio Romero Rodríguez
6.1. Conclusiones
El objetivo de este proyecto era automatizar un vehículo de dos ruedas, incorporando
un motor para la dirección y otro para la tracción, ambos controlados mediante un PCB
de potencia. Para poder medir los parámetros se dispone de tres sensores. Para medir
el Yaw y Roll se tiene una unidad de medición inercial (Arduimu), para medir la
velocidad, un encoder, y para medir el ángulo de la dirección, un potenciómetro que
forma parte, junto con el motor de la dirección, de un servo de potencia. Toda la
información de los sensores es recogida por la tarjeta Arduino y enviada a su vez a
Matlab. Al mismo tiempo, en Matlab se generan las órdenes necesarias para comandar
tanto el servo como el motor de la tracción.
Las pruebas realizadas sobre los actuadores han sido satisfactorias. Se ha logrado
construir un vehículo autónomo que está dotado con la capacidad de avanzar y de una
dirección capaz de proporcionar el par necesario para su control de la estabilidad.
Todo ello gestionado a partir de Matlab, tal cual era la idea original y la cual se ha
llevado a cabo.
Con la realización de este proyecto se han puesto en práctica una buena parte de los
conocimientos adquiridos durante los años de formación en la titulación de Ingeniería
Industrial, desde de materiales y técnicas de soldado adecuadas, como de electrónica
de potencia a la hora del control en cargas de alto consumo, pasando por la
aplicaciones de automática y control. Me ha aportado un punto de vista analítico a la
hora de reconocer como utilizar los conocimientos adquiridos para resolver cierto
problema de la vida real, hecho que me servirá de experiencia a la hora de
desenvolverme como profesional de la rama electrónica y automática.
Diseño y construcción de un vehículo autónomo de dos ruedas 87
Honorio Romero Rodríguez
6.2. Líneas de desarrollos futuras
El vehículo construido cumple todas las expectativas del proyecto pero existirían
muchas mejoras posibles. La primera podría ser la modificación del tren de arrastre de
tal manera que el vehículo pudiera aumentar su velocidad por encima de su límite
crítico de estabilidad. Tomando un gráfico del estudio de la dinámica de una bicicleta
en la siguiente figura.
Figura 76. Dinámica de bicicleta
Alrededor de 5 m/s la mayoría de las bicicletas comunes adquieren autoestabilidad son
el control sobre la dirección. Por ello sería interesante obtener un arranque controlado
y llevar al vehículo a esas velocidades, pasando Arduino a ser una simple tarjeta de
adquisición de datos de la física dinámica de la bicicleta.
Por otra parte Arduimu tiene la posibilidad de la conexión de GPS, también sería
interesante poder controlar el avance ya no solo desde el punto de vista de la
estabilidad, sino también para hacer pasar la trayectoria por puntos determinados e
igualmente recoger los datos de velocidades, giros y todos los parámetros en dicho
recorrido.
Existen multitud de mejoras, podría también añadirse un sónar, el cual podría usarse
para evitar obstáculos.
Para terminar, puesto que la bicicleta arranca desde velocidad nula, sería necesario
que estuviera provista de unas ruedas plegables y abatibles en función de esta
velocidad.
Diseño y construcción de un vehículo autónomo de dos ruedas 88
Honorio Romero Rodríguez
Parte VII
Bibliografía y referencias
Diseño y construcción de un vehículo autónomo de dos ruedas 89
Honorio Romero Rodríguez
7.1. Bibliografía
[1] http://es.wikipedia.org/wiki/Karl_Drais
http://es.wikipedia.org/wiki/Draisine
http://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics [2] E. Carvallo, Theorie du mouvement du Monocycle et de la Bicyclette, Journal de L’Ecole Polytechnique, Series 2, Part 1, Volume 5, Çerceau et Monocyle", 1900. [3] F. J. W. Whipple, Stability of the motion of a bicycle, Quarterly Journal of mathematics. Cornell Univ., Ithaca, NY, 1899. [4] http://en.wikipedia.org/wiki/David_E._H._Jones [5] J. D. G. Kooijman, J. P. Meijaard, J. M. Papadopoulos, A. Ruina y A. L. Schwab, A bicycle can be self-stable without gyroscopic or caster effects, Science Magazine, April15, 2011.
[6] http://www.murata.com/ [7] http://code.google.com/p/ardu-imu/wiki/Hardware [8] http://arduino.cc/en/Reference/HomePage http://arduino.cc/it/Reference/Board http://arduino.cc/en/Main/arduinoBoardUno
[9] D. W. Hart, Electrónica de potencia; traducción, Vuelapluma; revisión técnica, A. B.
Bautista.
[10] http://es.wikipedia.org/wiki/Puerto_serie
[11] http://es.wikipedia.org/wiki/Puerto_serie [12] http://es.wikipedia.org/wiki/UART [13] http://es.wikipedia.org/wiki/USB [14] http://es.wikipedia.org/wiki/I%C2%B2C [15] http://es.wikipedia.org/wiki/PWM
Diseño y construcción de un vehículo autónomo de dos ruedas 90
Honorio Romero Rodríguez
Parte VIII
Apéndice
Diseño y construcción de un vehículo autónomo de dos ruedas 91
Honorio Romero Rodríguez
8.1. Códigos
Para la parte de programación se ha usado el paquete ArduinoIO de Matlab
modificado para este sistema.
El código IDE de Arduino se ha construido utilizando códigos ya existentes y al mismo
tiempo modificando éstos mismos según necesidades.
Para la IMU se modificó su firmware para poder comunicar de la manera requerida con
Arduino vía I2C.
Las funciones y códigos con los siguientes
Install_arduino % This files installs the MATLAB support package for Arduino
(ArduinoIO package).
% Copyright 2011 The MathWorks, Inc.
% look for arduino.m wa=which('arduino.m','-all');
% make sure we are in the right folder and there are no other
arduino.m files if length(wa) < 1, msg=' Cannot find arduino.m, please run this file from the folder
containing arduino.m'; error(msg); end if length(wa) > 1, msg=' There is at least another arduino.m file in the path, please
delete any other versions before installing this one'; error(msg); end
% get the main arduino folder ap=wa1;ap=ap(1:end-10);
% Add target directories and save the updated path addpath(fullfile(ap,'')); addpath(fullfile(ap,'simulink','')); addpath(fullfile(ap,'examples','')); disp(' Libreria Arduino cargada');
result = savepath; if result==1 nl = char(10); msg = [' Unable to save updated MATLAB path (<a
href="http://www.mathworks.com/support/solutions/en/data/1-
9574H9/index.html?solution=1-9574H9">why?</a>)' nl ... ' On Windows, exit MATLAB, right-click on the MATLAB icon,
select "Run as administrator", and re-run install_arduino.m' nl ... ' On Linux, exit MATLAB, issue a command like this: sudo
chmod 777 usr/local/matlab/R2011a/toolbox/local/pathdef.m' nl ...
Diseño y construcción de un vehículo autónomo de dos ruedas 92
Honorio Romero Rodríguez
' (depending on where MATLAB is installed), and then re
open MATLAB and re-run install_arduino.m' nl ... ]; error(msg); else
disp(' OK '); end
clear wa ap result nl msg
Arduino.m
classdef arduino < handle
% This class defines an "arduino" object % Giampiero Campa, Aug 2010, Copyright 2009 The MathWorks, Inc.
properties (SetAccess=private,GetAccess=private) aser % Serial Connection pins % Pin Status Vector srvs % Servo Status Vector mspd % Motor Speed Status sspd % Servo Speed Status mots % Motor Server Running on the Arduino Board end
properties (Hidden=true) chks = false; % Checks serial connection before every
operation chkp = true; % Checks parameters before every operation end
methods
% constructor, connects to the board and creates an arduino
object function a=arduino(comPort)
% check nargin if nargin<1, comPort='DEMO'; disp('Note: a DEMO connection will be created'); disp('Use a the com port, e.g. ''COM5'' as input
argument to connect to the real board'); end
% check port
Diseño y construcción de un vehículo autónomo de dos ruedas 93
Honorio Romero Rodríguez
if ~ischar(comPort), error('The input argument must be a string, e.g.
''COM8'' '); end
% check if we are already connected if isa(a.aser,'serial') && isvalid(a.aser) &&
strcmpi(get(a.aser,'Status'),'open'), disp(['It looks like Arduino is already connected to
port ' comPort ]); disp('Delete the object to force disconnection'); disp('before attempting a connection to a different
port.'); return; end
% check whether serial port is currently used by MATLAB if ~isempty(instrfind('Port',comPort)), disp(['The port ' comPort ' is already used by
MATLAB']); disp(['If you are sure that Arduino is connected to '
comPort]); disp('then delete the object, execute:'); disp([' delete(instrfind(''Port'',''' comPort
'''))']); disp('to delete the port, disconnect the cable,
reconnect it,'); disp('and then create a new arduino object'); error(['Port ' comPort ' already used by MATLAB']); end
% define serial object a.aser=serial(comPort,'BaudRate',115200);
% connection if strcmpi(get(a.aser,'Port'),'DEMO'), % handle demo mode
fprintf(1,'Demo mode connection .'); for i=1:5, fprintf(1,'.'); pause(1); end fprintf(1,'\n'); pause(1);
% chk is equal to 3, (general server running) chk=3;
else % actual connection
% open port try fopen(a.aser); catch ME, disp(ME.message) delete(a); error(['Could not open port: ' comPort]); end
Diseño y construcción de un vehículo autónomo de dos ruedas 94
Honorio Romero Rodríguez
% it takes several seconds before any operation could
be attempted
fprintf(1,'Attempting connection .'); for i=1:7, fprintf(1,'.'); pause(1); end fprintf(1,'\n');
% query script type fwrite(a.aser,[57 57],'uchar'); chk=fscanf(a.aser,'%d');
% exit if there was no answer if isempty(chk) delete(a); error('Connection unsuccessful, please make sure
that the Arduino is powered on, running either srv.pde, adiosrv.pde or
mororsrv.pde, and that the board is connected to the indicated serial
port. You might also try to unplug and re-plug the USB cable before
attempting a reconnection.'); end
end
% check returned value if chk==1, disp('Basic I/O Script detected !'); elseif chk==2, disp('Motor Shield Script detected !'); elseif chk==3, disp('General Script detected !'); else delete(a); error('Unknown Script. Please make sure that either
adiosrv.pde or motorsrv.pde are running on the Arduino'); end
% sets a.mots flag a.mots=chk-1;
% set a.aser tag a.aser.Tag='ok';
% initialize pin vector (-1 is unassigned, 0 is input, 1
is output) a.pins=-1*ones(1,19);
% initialize servo vector (-1 is unknown, 0 is detached, 1
is attached) a.srvs=0*ones(1,2);
% initialize motor vector (0 to 255 is the speed) a.mspd=0*ones(1,4);
% initialize stepper vector (0 to 255 is the speed) a.sspd=0*ones(1,2);
Diseño y construcción de un vehículo autónomo de dos ruedas 95
Honorio Romero Rodríguez
% notify successful installation disp('Arduino successfully connected !');
end % arduino
% distructor, deletes the object function delete(a)
% if it is a serial, valid and open then close it if isa(a.aser,'serial') && isvalid(a.aser) &&
strcmpi(get(a.aser,'Status'),'open'), if ~isempty(a.aser.Tag), try % trying to leave it in a known unharmful
state for i=2:19, a.pinMode(i,'output'); a.digitalWrite(i,0); a.pinMode(i,'input'); end catch ME % disp but proceed anyway disp(ME.message); disp('Proceeding to deletion anyway'); end
end fclose(a.aser); end
% if it's an object delete it if isobject(a.aser), delete(a.aser); end
end % delete
% disp, displays the object function disp(a) % display if isvalid(a), if isa(a.aser,'serial') && isvalid(a.aser), disp(['<a href="matlab:help arduino">arduino</a>
object connected to ' a.aser.port ' port']); if a.mots==2, disp('General Shield Server running on the
arduino board'); disp(' '); a.servoStatus a.motorSpeed a.stepperSpeed disp(' '); disp('Servo Methods: <a href="matlab:help
servoStatus">servoStatus</a> <a href="matlab:help
servoAttach">servoAttach</a> <a href="matlab:help
servoDetach">servoDetach</a> <a href="matlab:help
servoRead">servoRead</a> <a href="matlab:help
servoWrite">servoWrite</a>');
Diseño y construcción de un vehículo autónomo de dos ruedas 96
Honorio Romero Rodríguez
disp('DC Motors and Stepper Methods: <a
href="matlab:help motorSpeed">motorSpeed</a> <a href="matlab:help
motorRun">motorRun</a> <a href="matlab:help
stepperSpeed">stepperSpeed</a> <a href="matlab:help
stepperStep">stepperStep</a>'); disp(' '); a.pinMode disp(' '); disp('Pin IO Methods: <a href="matlab:help
pinMode">pinMode</a> <a href="matlab:help digitalRead">digitalRead</a>
<a href="matlab:help digitalWrite">digitalWrite</a> <a
href="matlab:help analogRead">analogRead</a> <a href="matlab:help
analogWrite">analogWrite</a>'); elseif a.mots==1, disp('Motor Shield Server running on the
arduino board'); disp(' '); a.servoStatus a.motorSpeed a.stepperSpeed disp(' '); disp('Servo Methods: <a href="matlab:help
servoStatus">servoStatus</a> <a href="matlab:help
servoAttach">servoAttach</a> <a href="matlab:help
servoDetach">servoDetach</a> <a href="matlab:help
servoRead">servoRead</a> <a href="matlab:help
servoWrite">servoWrite</a>'); disp('DC Motors and Stepper Methods: <a
href="matlab:help motorSpeed">motorSpeed</a> <a href="matlab:help
motorRun">motorRun</a> <a href="matlab:help
stepperSpeed">stepperSpeed</a> <a href="matlab:help
stepperStep">stepperStep</a>'); else disp('IO Server running on the arduino
board'); disp(' '); a.pinMode disp(' '); disp('Pin IO Methods: <a href="matlab:help
pinMode">pinMode</a> <a href="matlab:help digitalRead">digitalRead</a>
<a href="matlab:help digitalWrite">digitalWrite</a> <a
href="matlab:help analogRead">analogRead</a> <a href="matlab:help
analogWrite">analogWrite</a>'); end disp(' '); else disp('<a href="matlab:help arduino">arduino</a>
object connected to an invalid serial port'); disp('Please delete the arduino object'); disp(' '); end else disp('Invalid <a href="matlab:help
arduino">arduino</a> object'); disp('Please clear the object and instantiate
another one'); disp(' '); end end
% pin mode, changes pin mode
Diseño y construcción de un vehículo autónomo de dos ruedas 97
Honorio Romero Rodríguez
function pinMode(a,pin,str)
% a.pinMode(pin,str); specifies the pin mode of a digital
pins. % The first argument before the function name, a, is the
arduino object. % The first argument, pin, is the number of the digital
pin (2 to 19). % The second argument, str, is a string that can be
'input' or 'output', % Called with one argument, as a.pin(pin) it returns the
mode of % the digital pin, called without arguments, prints the
mode of all the % digital pins. Note that the digital pins from 0 to 13
are located on % the upper right part of the board, while the digital
pins from 14 to 19 % are better known as "analog input" pins and are located
in the lower % right corner of the board. % % Examples: % a.pinMode(11,'output') % sets digital pin #11 as output % a.pinMode(10,'input') % sets digital pin #10 as input % val=a.pinMode(10); % returns the status of digital
pin #10 % a.pinMode(5); % prints the status of digital
pin #5 % a.pinMode; % prints the status of all pins %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin>3, error('This function cannot have more than 3
arguments, object, pin and str'); end
% if pin argument is there check it if nargin>1, errstr=arduino.checknum(pin,'pin number',2:19); if ~isempty(errstr), error(errstr); end end
% if str argument is there check it if nargin>2, errstr=arduino.checkstr(str,'pin
mode','input','output'); if ~isempty(errstr), error(errstr); end end
end
% perform the requested action
Diseño y construcción de un vehículo autónomo de dos ruedas 98
Honorio Romero Rodríguez
if nargin==3,
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% CHANGE PIN MODE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% assign value if lower(str(1))=='o', val=1; else val=0; end
if strcmpi(get(a.aser,'Port'),'DEMO'), % handle demo mode here
% minimum digital output delay pause(0.0014);
else % do the actual action here
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode, pin and value fwrite(a.aser,[48 97+pin 48+val],'uchar');
end
% detach servo 1 or 2 if pins 10 or 9 are used if pin==10 || pin==9, a.servoDetach(11-pin); end
% store 0 for input and 1 for output a.pins(pin)=val;
elseif nargin==2, % print pin mode for the requested pin
mode='UNASSIGNED','set as INPUT','set as OUTPUT'; disp(['Digital Pin ' num2str(pin) ' is currently '
mode2+a.pins(pin)]);
else % print pin mode for each pin
mode='UNASSIGNED','set as INPUT','set as OUTPUT'; for i=2:19; disp(['Digital Pin ' num2str(i,'%02d') ' is
currently ' mode2+a.pins(i)]); end
end
Diseño y construcción de un vehículo autónomo de dos ruedas 99
Honorio Romero Rodríguez
end % pinmode
% digital read function val=digitalRead(a,pin)
% val=a.digitalRead(pin); performs digital input on a
given arduino pin. % The first argument before the function name, a, is the
arduino object. % The argument pin, is the number of the digital pin (2 to
19) % where the digital input needs to be performed. Note that
the digital pins % from 0 to 13 are located on the upper right part of the
board, while the % digital pins from 14 to 19 are better known as "analog
input" pins and % are located in the lower right corner of the board. % % Example: % val=a.digitalRead(4); % reads pin #4 %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin~=2, error('Function must have the "pin" argument'); end
% check pin errstr=arduino.checknum(pin,'pin number',2:19); if ~isempty(errstr), error(errstr); end
end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% PERFORM DIGITAL INPUT
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO'), % handle demo mode
% minimum digital input delay pause(0.0074);
% output 0 or 1 randomly val=round(rand);
Diseño y construcción de un vehículo autónomo de dos ruedas 100
Honorio Romero Rodríguez
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode and pin fwrite(a.aser,[49 97+pin],'uchar');
% get value val=fscanf(a.aser,'%d');
end
end % digitalread
% digital write function digitalWrite(a,pin,val)
% a.digitalWrite(pin,val); performs digital output on a
given pin. % The first argument before the function name, a, is the
arduino object. % The second argument, pin, is the number of the digital
pin (2 to 19) % where the digital output needs to be performed. % The third argument, val, is the value (either 0 or 1)
for the output % Note that the digital pins from 0 to 13 are located on
the upper right part % of the board, while the digital pins from 14 to 19 are
better known as % "analog input" pins and are located in the lower right
corner of the board. % % Examples: % a.digitalWrite(13,1); % sets pin #13 high % a.digitalWrite(13,0); % sets pin #13 low %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin~=3, error('Function must have the "pin" and "val"
arguments'); end
% check pin errstr=arduino.checknum(pin,'pin number',2:19); if ~isempty(errstr), error(errstr); end
% check val
Diseño y construcción de un vehículo autónomo de dos ruedas 101
Honorio Romero Rodríguez
errstr=arduino.checknum(val,'value',0:1); if ~isempty(errstr), error(errstr); end
% get object name if isempty(inputname(1)), name='object'; else
name=inputname(1); end
% pin should be configured as output if a.pins(pin)~=1, warning('MATLAB:Arduino:digitalWrite',['If digital
pin ' num2str(pin) ' is set as input, digital output takes place only
after using ' name' '.pinMode(' num2str(pin) ',''output''); ']); end
end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% PERFORM DIGITAL OUTPUT
%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO'), % handle demo mode
% minimum digital output delay pause(0.0014);
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode, pin and value fwrite(a.aser,[50 97+pin 48+val],'uchar');
end
end % digitalwrite
% analog read function val=analogRead(a,pin)
% val=a.analogRead(pin); Performs analog input on a given
arduino pin. % The first argument before the function name, a, is the
arduino object. % The second argument, pin, is the number of the analog
input pin (0 to 5) % where the analog input needs to be performed. The
returned value, val, % ranges from 0 to 1023, with 0 corresponding to an input
voltage of 0 volts,
Diseño y construcción de un vehículo autónomo de dos ruedas 102
Honorio Romero Rodríguez
% and 1023 to a reference value that is typically 5 volts
(this voltage can % be set up by the analogReference function). Therefore,
assuming a range % from 0 to 5 V the resolution is .0049 volts (4.9 mV) per
unit. % Note that the analog input pins 0 to 5 are also known as
digital pins % from 14 to 19, and are located on the lower right corner
of the board. % Specifically, analog input pin 0 corresponds to digital
pin 14, and analog % input pin 5 corresponds to digital pin 19. Performing
analog input does % not affect the digital state (high, low, digital input)
of the pin. % % Example: % val=a.analogRead(0); % reads analog input pin # 0 %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin~=2, error('Function must have the "pin" argument'); end
% check pin errstr=arduino.checknum(pin,'analog input pin
number',0:5); if ~isempty(errstr), error(errstr); end
end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% PERFORM ANALOG INPUT
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO'), % handle demo mode
% minimum analog input delay pause(0.0074);
% output a random value between 0 and 1023 val=round(1023*rand);
else
Diseño y construcción de un vehículo autónomo de dos ruedas 103
Honorio Romero Rodríguez
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode and pin fwrite(a.aser,[51 97+pin],'uchar');
% get value val=fscanf(a.aser,'%d');
end
end % analogread
% function analog write function analogWrite(a,pin,val)
% a.analogWrite(pin,val); Performs analog output on a
given arduino pin. % The first argument before the function name, a, is the
arduino object. % The first argument, pin, is the number of the DIGITAL
pin where the analog % (PWM) output needs to be performed. Allowed pins for AO
are 3,5,6,9,10,11 % The second argument, val, is the value from 0 to 255 for
the level of % analog output. Note that the digital pins from 0 to 13
are located on the % upper right part of the board. % % Examples: % a.analogWrite(11,90); % sets pin #11 to 90/255 % a.analogWrite(3,10); % sets pin #3 to 10/255 %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin~=3, error('Function must have the "pin" and "val"
arguments'); end
% check pin errstr=arduino.checknum(pin,'pwm pin number',[3 5 6 9
10 11]); if ~isempty(errstr), error(errstr); end
% check val errstr=arduino.checknum(val,'analog output
level',0:255);
Diseño y construcción de un vehículo autónomo de dos ruedas 104
Honorio Romero Rodríguez
if ~isempty(errstr), error(errstr); end
% get object name if isempty(inputname(1)), name='object'; else
name=inputname(1); end
% pin should be configured as output if a.pins(pin)~=1, warning('MATLAB:Arduino:analogWrite',['If digital
pin ' num2str(pin) ' is set as input, pwm output takes place only
after using ' name '.pinMode(' num2str(pin) ',''output''); ']); end
end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% PERFORM ANALOG OUTPUT
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO'), % handle demo mode
% minimum analog output delay pause(0.0014);
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode, pin and value fwrite(a.aser,[52 97+pin val],'uchar');
end
end % analogwrite
% servo attach function servoAttach(a,num)
% a.servoAttach(num); attaches a servo to the
corresponding pwm pin. % The first argument before the function name, a, is the
arduino object. % The second argument, num, is the number of the servo,
which can be either 1 % (top servo, uses digital pin 10 for pwm), or 2 (bottom
servo, uses digital % pin 9 for pwm). % % Example:
Diseño y construcción de un vehículo autónomo de dos ruedas 105
Honorio Romero Rodríguez
% a.servoAttach(1); % attach servo #1 %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin~=2, error('Function must have the "num" argument'); end
% check servo number errstr=arduino.checknum(num,'servo number',[1 2]); if ~isempty(errstr), error(errstr); end
end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% ATTACH SERVO
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO') || a.mots==0, % handle demo mode
% minimum digital output delay pause(0.0014);
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode, num and value (1 for attach) fwrite(a.aser,[54 96+num 48+1],'uchar');
end
% store the servo statur a.srvs(num)=1;
% update pin status to unassigned a.pins(11-num)=-1;
end % servoattach
% servo detach function servoDetach(a,num)
Diseño y construcción de un vehículo autónomo de dos ruedas 106
Honorio Romero Rodríguez
% a.servoDetach(num); detaches a servo from its
corresponding pwm pin. % The first argument before the function name, a, is the
arduino object. % The second argument, num, is the number of the servo,
which can be either 1 % (top servo, uses digital pin 10 for pwm), or 2 (bottom
servo, uses digital % pin 9 for pwm). % % Examples: % a.servoDetach(1); % detach servo #1 %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin~=2, error('Function must have the "num" argument'); end
% check servo number errstr=arduino.checknum(num,'servo number',[1 2]); if ~isempty(errstr), error(errstr); end
end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% DETACH SERVO
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO') || a.mots==0, % handle demo mode
% minimum digital output delay pause(0.0014);
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode, num and value (0 for detach) fwrite(a.aser,[54 96+num 48+0],'uchar');
end
Diseño y construcción de un vehículo autónomo de dos ruedas 107
Honorio Romero Rodríguez
a.srvs(num)=0;
end % servodetach
% servo status function val=servoStatus(a,num)
% a.servoStatus(num); Reads the status of a servo
(attached/detached) % The first argument before the function name, a, is the
arduino object. % The second argument, num, is the number of the servo,
which can be either 1 % (top servo, uses digital pin 10 for pwm), or 2 (bottom
servo, % uses digital pin 9 for pwm). % The returned value is either 1 (servo attached) or 0
(servo detached), % Called without output arguments, the function prints a
string specifying % the status of the servo. Called without input arguments,
the function % either returns the status vector or prints the status of
each servo. % % Examples: % val=a.servoStatus(1); % return the status of servo #1 % a.servoStatus(1); % prints the status of servo #1 % a.servoStatus; % prints the status of both servos %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check nargin if a.chkp is true if a.chkp, if nargin>2, error('Function cannot have more than one argument
(servo number) beyond the object name'); end end
% with no arguments calls itself recursively for both
servos if nargin==1, if nargout>0, val(1)=a.servoStatus(1); val(2)=a.servoStatus(2); return else a.servoStatus(1); a.servoStatus(2); return end end
% check servo number if a.chkp is true if a.chkp, errstr=arduino.checknum(num,'servo number',[1 2]);
Diseño y construcción de un vehículo autónomo de dos ruedas 108
Honorio Romero Rodríguez
if ~isempty(errstr), error(errstr); end end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% ASK SERVO STATUS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO') || a.mots==0, % handle demo mode
% minimum digital input delay pause(0.0074);
% gets value from the servo state vector val=a.srvs(num);
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode and num fwrite(a.aser,[53 96+num],'uchar');
% get value val=fscanf(a.aser,'%d');
end
% updates the servo state vector a.srvs(num)=val;
if nargout==0, str='DETACHED','ATTACHED'; disp(['Servo ' num2str(num) ' is ' str1+val]); clear val return end
end % servostatus
% servo read function val=servoRead(a,num)
% val=a.servoRead(num); reads the angle of a given servo. % The first argument before the function name, a, is the
arduino object. % The second argument, num, is the number of the servo,
which can be either
Diseño y construcción de un vehículo autónomo de dos ruedas 109
Honorio Romero Rodríguez
% 1 (top servo, uses digital pin 10 for pwm), or 2 (bottom
servo, uses % digital pin 9 for pwm). The returned value is the angle
in degrees, % typically from 0 to 180. Returns Random results if motor
shield is not % connected. % % Example: % val=a.servoRead(1); % reads angle from servo #1 %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin~=2, error('Function must have the servo number
argument'); end
% check servo number errstr=arduino.checknum(num,'servo number',[1 2]); if ~isempty(errstr), error(errstr); end
% check status if a.srvs(num)~=1, error(['Servo ' num2str(num) ' is not attached,
please use a.servoAttach(' num2str(num) ') to attach it']); end
end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% READ SERVO ANGLE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO') || a.mots==0, % handle demo mode
% minimum analog input delay pause(0.0074);
% output a random value between 0 and 180 val=round(180*rand);
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open');
Diseño y construcción de un vehículo autónomo de dos ruedas 110
Honorio Romero Rodríguez
if ~isempty(errstr), error(errstr); end end
% send mode and num fwrite(a.aser,[55 96+num],'uchar');
% get value val=fscanf(a.aser,'%d');
end
end % servoread
% servo write function servoWrite(a,num,val)
% a.servoWrite(num,val); writes an angle on a given servo. % The first argument before the function name, a, is the
arduino object. % The second argument, num, is the number of the servo,
which can be % either 1 (top servo, uses digital pin 10 for pwm), or 2
(bottom servo, % uses digital pin 9 for pwm). The third argument is the
angle in degrees, % typically from 0 to 180. Returns Random results if motor
shield is not % connected. % % Example: % a.servoWrite(1,45); % rotates servo #1 of 45 degrees %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin~=3, error('Function must have the servo number and
angle arguments'); end
% check servo number errstr=arduino.checknum(num,'servo number',[1 2]); if ~isempty(errstr), error(errstr); end
% check angle value errstr=arduino.checknum(val,'angle',0:180); if ~isempty(errstr), error(errstr); end
% check status if a.srvs(num)~=1, error(['Servo ' num2str(num) ' is not attached,
please use a.servoAttach(' num2str(num) ') to attach it']); end
Diseño y construcción de un vehículo autónomo de dos ruedas 111
Honorio Romero Rodríguez
end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% WRITE ANGLE TO SERVO
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO') || a.mots==0, % handle demo mode
% minimum analog output delay pause(0.0014);
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode, num and value fwrite(a.aser,[56 96+num val],'uchar');
end
end % servowrite
% motor speed function val=motorSpeed(a,num,val)
% val=a.motorSpeed(num,val); sets the speed of a DC motor. % The first argument before the function name, a, is the
arduino object. % The second argument, num, is the number of the motor,
which can go % from 1 to 4 (the motor ports are numbered on the motor
shield). % The third argument is the speed from 0 (stopped) to 255
(maximum), note % that depending on the motor speeds of at least 60 might
be necessary % to actually run it. Called with one argument, as
a.motorSpeed(num), % it returns the speed at which the given motor is set to
run. If there % is no output argument it prints the speed of the motor. % Called without arguments, itprints the speed of each
motor. % Note that you must use the command a.motorRun to
actually run % the motor at the given speed, either forward or
backwards. % Returns Random results if motor shield is not connected. %
Diseño y construcción de un vehículo autónomo de dos ruedas 112
Honorio Romero Rodríguez
% Examples: % a.motorSpeed(4,200) % sets speed of motor 4 as
200/255 % val=a.motorSpeed(1); % returns the speed of motor 1 % a.motorSpeed(3); % prints the speed of motor 3 % a.motorSpeed; % prints the speed of all
motors %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin>3, error('This function cannot have more than 3
arguments, arduino object, motor number and speed'); end
% if motor number is there check it if nargin>1, errstr=arduino.checknum(num,'motor number',1:4); if ~isempty(errstr), error(errstr); end end
% if speed argument is there check it if nargin>2, errstr=arduino.checknum(val,'speed',0:255); if ~isempty(errstr), error(errstr); end end
end
% perform the requested action if nargin==3,
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% SET MOTOR SPEED
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO') || a.mots==0, % handle demo mode
% minimum analog output delay pause(0.0014);
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end
Diseño y construcción de un vehículo autónomo de dos ruedas 113
Honorio Romero Rodríguez
end
% send mode, num and value fwrite(a.aser,[65 48+num val],'uchar');
end
% store speed value in case it needs to be retrieved a.mspd(num)=val;
% clear val if is not needed as output if nargout==0, clear val; end
elseif nargin==2,
if nargout==0, % print speed value disp(['The speed of motor number ' num2str(num) '
is set to: ' num2str(a.mspd(num)) ' over 255']); else % return speed value val=a.mspd(num); end
else
if nargout==0, % print speed value for each motor for num=1:4, disp(['The speed of motor number '
num2str(num) ' is set to: ' num2str(a.mspd(num)) ' over 255']); end else % return speed values val=a.mspd; end
end
end % motorspeed
% motor run function motorRun(a,num,dir)
% a.motorRun(num,dir); runs a given DC motor. % The first argument before the function name, a, is the
arduino object. % The second argument, num, is the number of the motor,
which can go % from 1 to 4 (the motor ports are numbered on the motor
shield). % The third argument, dir, should be a string that can be
'forward' % (runs the motor forward) 'backward' (runs the motor
backward) % or 'release', (stops the motor). Note that since version
3.0,
Diseño y construcción de un vehículo autónomo de dos ruedas 114
Honorio Romero Rodríguez
% a +1 is interpreted as 'forward', a 0 is interpreted % as 'release', and a -1 is interpreted as 'backward'. % Returns Random results if motor shield is not connected. % % Examples: % a.motorRun(1,'forward'); % runs motor 1 forward % a.motorRun(3,'backward'); % runs motor 3 backward % a.motorRun(2,-1); % runs motor 2 backward % a.motorRun(1,'release'); % releases motor 1 %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin~=3, error('Function must have 3 arguments, object,
motor number and direction'); end
% check motor number errstr=arduino.checknum(num,'motor number',1:4); if ~isempty(errstr), error(errstr); end
end
% allows for direction to be set by 1,0,-1 if isnumeric(dir) && isscalar(dir), switch dir case 1, dir='forward'; case 0, dir='release'; case -1, dir='backward'; end end
% check direction if a.chkp is true if a.chkp,
errstr=arduino.checkstr(dir,'direction','forward','backward','release
'); if ~isempty(errstr), error(errstr); end end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% RUN THE MOTOR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO') || a.mots ==0,
Diseño y construcción de un vehículo autónomo de dos ruedas 115
Honorio Romero Rodríguez
% handle demo mode
% minimum analog output delay pause(0.0014);
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode, num and value fwrite(a.aser,[66 48+num abs(dir(1))],'uchar');
end
end % motorrun
% stepper speed function val=stepperSpeed(a,num,val)
% val=a.stepperSpeed(num,val); sets the speed of a given
stepper motor % The first argument before the function name, a, is the
arduino object. % The second argument, num, is the number of the stepper
motor, % which can go from 1 to 4 (the motor ports are numbered
on the motor shield). % The third argument is the RPM speed from 1 (minimum) to
255 (maximum). % Called with one argument, as a.stepperSpeed(num), it
returns the % speed at which the given motor is set to run. If there
is no output % argument it prints the speed of the stepper motor. % Called without arguments, itprints the speed of each
stepper motor. % Note that you must use the command a.stepperStep to
actually run % the motor at the given speed, either forward or
backwards (or release % it). Returns Random results if motor shield is not
connected. % % Examples: % a.stepperSpeed(2,50) % sets speed of stepper 2 as
50 rpm % val=a.stepperSpeed(1); % returns the speed of
stepper 1 % a.stepperSpeed(2); % prints the speed of stepper
2 % a.stepperSpeed; % prints the speed of both
steppers %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Diseño y construcción de un vehículo autónomo de dos ruedas 116
Honorio Romero Rodríguez
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin>3, error('This function cannot have more than 3
arguments, object, stepper number and speed'); end
% if stepper number is there check it if nargin>1, errstr=arduino.checknum(num,'stepper number',1:2); if ~isempty(errstr), error(errstr); end end
% if speed argument is there check it if nargin>2, errstr=arduino.checknum(val,'speed',0:255); if ~isempty(errstr), error(errstr); end end
end
% perform the requested action if nargin==3,
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% SET STEPPER SPEED
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO') || a.mots==0, % handle demo mode
% minimum analog output delay pause(0.0014);
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode, num and value fwrite(a.aser,[67 48+num val],'uchar');
end
% store speed value in case it needs to be retrieved a.sspd(num)=val;
Diseño y construcción de un vehículo autónomo de dos ruedas 117
Honorio Romero Rodríguez
% clear val if is not needed as output if nargout==0, clear val; end
elseif nargin==2,
if nargout==0, % print speed value disp(['The speed of stepper number ' num2str(num)
' is set to: ' num2str(a.sspd(num)) ' over 255']); else % return speed value val=a.sspd(num); end
else
if nargout==0, % print speed value for each stepper for num=1:2, disp(['The speed of stepper number '
num2str(num) ' is set to: ' num2str(a.sspd(num)) ' over 255']); end else % return speed values val=a.sspd; end
end
end % stepperspeed
% stepper step function stepperStep(a,num,dir,sty,steps)
% a.stepperStep(num,dir,sty,steps); rotates a given
stepper motor % The first argument before the function name, a, is the
arduino object. % The second argument, num, is the number of the stepper
motor, which is % either 1 or 2. The third argument, the direction, is a
string that can % be 'forward' (runs the motor forward) 'backward' (runs
the motor backward) % or 'release', (stops and releases the motor). Note that
since version 3.0, % a +1 is interpreted as 'forward', a 0 is interpreted as
'release', % and a -1 is interpreted as 'backward'. Unless the
direction is 'release', % then two more argument are needed: the fourth one is the
style, % which is a string specifying the style of the motion,
and can be 'single' % (only one coil activated at a time), 'double' (2 coils
activated, gives % an higher torque and power consumption) 'interleave',
(alternates between
Diseño y construcción de un vehículo autónomo de dos ruedas 118
Honorio Romero Rodríguez
% single and double to get twice the resolution and half
the speed), and % 'microstep' (the coils are driven in PWM for a smoother
motion). % The final argument is the number of steps that the motor
has % to complete. % Returns Random results if motor shield is not connected. % % Examples: % % rotates stepper 1 forward of 100 steps in interleave
mode % a.stepperStep(1,'forward','double',100); % % rotates stepper 2 forward of 50 steps in double mode % a.stepperStep(1,'forward','double',50); % % rotates stepper 2 backward of 50 steps in single mode % a.stepperStep(2,'backward','single',50); %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin>5 || nargin <3, error('Function must have at least 3 and no more
than 5 arguments'); end
% check stepper number errstr=arduino.checknum(num,'stepper number',1:2); if ~isempty(errstr), error(errstr); end
end
% allows for direction to be set by 1,0,-1 if isnumeric(dir) && isscalar(dir), switch dir case 1, dir='forward'; case 0, dir='release'; case -1, dir='backward'; end end
% check arguments if a.chkp is true if a.chkp,
% check direction
errstr=arduino.checkstr(dir,'direction','forward','backward','release
'); if ~isempty(errstr), error(errstr); end
% if it is not released must have all arguments if ~strcmpi(dir,'release') && nargin~=5,
Diseño y construcción de un vehículo autónomo de dos ruedas 119
Honorio Romero Rodríguez
error('Either the motion style or the number of
steps are missing'); end
% can't move forward or backward if speed is set to
zero if ~strcmpi(dir,'release') && a.stepperSpeed(num)<1, error('The stepper speed has to be greater than
zero for the stepper to move'); end
% check motion style if nargin>3, % check direction errstr=arduino.checkstr(sty,'motion
style','single','double','interleave','microstep'); if ~isempty(errstr), error(errstr); end else sty='single'; end
% check number of steps if nargin==5, errstr=arduino.checknum(steps,'number of
steps',0:255); if ~isempty(errstr), error(errstr); end else steps=0; end
end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%%%%%%% ROTATE THE STEPPER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO') || a.mots==0, % handle demo mode
% minimum analog output delay pause(0.0014);
else
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
% send mode, num and value fwrite(a.aser,[68 48+num abs(dir(1)) abs(sty(1))
steps],'uchar');
Diseño y construcción de un vehículo autónomo de dos ruedas 120
Honorio Romero Rodríguez
end
end % stepperstep
% function analog reference function analogReference(a,str)
% a.analogReference(str); Changes voltage reference on
analog input pins % The first argument before the function name, a, is the
arduino object. % The second argument, str, is one of these strings:
'default', 'internal' % or 'external'. This sets the reference voltage used at
the top of the % input ranges. % % Examples: % a.analogReference('default'); % sets default reference % a.analogReference('internal'); % sets internal reference % a.analogReference('external'); % sets external reference %
%%%%%%%%%%%%%%%%%%%%%%%%% ARGUMENT CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check arguments if a.chkp is true if a.chkp,
% check nargin if nargin~=2, error('Function must have the "reference"
argument'); end
% check val
errstr=arduino.checkstr(str,'reference','default','internal','externa
l'); if ~isempty(errstr), error(errstr); end
end
% check a.aser for validity if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'valid'); if ~isempty(errstr), error(errstr); end end
%%%%%%%%%%%%%%%%%%%% CHANGE ANALOG INPUT REFERENCE
%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmpi(get(a.aser,'Port'),'DEMO'), % handle demo mode
% minimum analog output delay pause(0.0014);
else
Diseño y construcción de un vehículo autónomo de dos ruedas 121
Honorio Romero Rodríguez
% check a.aser for openness if a.chks is true if a.chks, errstr=arduino.checkser(a.aser,'open'); if ~isempty(errstr), error(errstr); end end
if lower(str(1))=='e', num=2; elseif lower(str(1))=='i', num=1; else num=0; end
% send mode, pin and value fwrite(a.aser,[82 48+num],'uchar');
end
end % analogreference
end % methods
methods (Static) % static methods
function errstr=checknum(num,description,allowed)
% errstr=arduino.checknum(num,description,allowed); Checks
numeric argument. % This function checks the first argument, num, described
in the string % given as a second argument, to make sure that it is
real, scalar, % and that it is equal to one of the entries of the vector
of allowed % values given as a third argument. If the check is
successful then the % returned argument is empty, otherwise it is a string
specifying % the type of error.
% initialize error string errstr=[];
% check num for type if ~isnumeric(num), errstr=['The ' description ' must be numeric']; return end
% check num for size if numel(num)~=1, errstr=['The ' description ' must be a scalar']; return end
% check num for realness if ~isreal(num), errstr=['The ' description ' must be a real value']; return
Diseño y construcción de un vehículo autónomo de dos ruedas 122
Honorio Romero Rodríguez
end
% check num against allowed values if ~any(allowed==num),
% form right error string if numel(allowed)==1, errstr=['Unallowed value for ' description ', the
value must be exactly ' num2str(allowed(1))]; elseif numel(allowed)==2, errstr=['Unallowed value for ' description ', the
value must be either ' num2str(allowed(1)) ' or '
num2str(allowed(2))]; elseif max(diff(allowed))==1, errstr=['Unallowed value for ' description ', the
value must be an integer going from ' num2str(allowed(1)) ' to '
num2str(allowed(end))]; else errstr=['Unallowed value for ' description ', the
value must be one of the following: ' mat2str(allowed)]; end
end
end % checknum
function errstr=checkstr(str,description,allowed)
% errstr=arduino.checkstr(str,description,allowed); Checks
string argument. % This function checks the first argument, str, described
in the string % given as a second argument, to make sure that it is a
string, and that % its first character is equal to one of the entries in
the cell of % allowed characters given as a third argument. If the
check is successful % then the returned argument is empty, otherwise it is a
string specifying % the type of error.
% initialize error string errstr=[];
% check string for type if ~ischar(str), errstr=['The ' description ' argument must be a
string']; return end
% check string for size if numel(str)<1, errstr=['The ' description ' argument cannot be
empty']; return end
Diseño y construcción de un vehículo autónomo de dos ruedas 123
Honorio Romero Rodríguez
% check str against allowed values if ~any(strcmpi(str,allowed)),
% make sure this is a hozizontal vector allowed=allowed(:)';
% add a comma at the end of each value for i=1:length(allowed)-1, allowedi=['''' allowedi ''', ']; end
% form error string errstr=['Unallowed value for ' description ', the
value must be either: ' allowed1:end-1 'or ''' allowedend '''']; return end
end % checkstr
function errstr=checkser(ser,chk)
% errstr=arduino.checkser(ser,chk); Checks serial
connection argument. % This function checks the first argument, ser, to make
sure that either: % 1) it is a valid serial connection (if the second
argument is 'valid') % 3) it is open (if the second argument is 'open') % If the check is successful then the returned argument is
empty, % otherwise it is a string specifying the type of error.
% initialize error string errstr=[];
% check serial connection switch lower(chk),
case 'valid',
% make sure is valid if ~isvalid(ser), disp('Serial connection invalid, please
recreate the object to reconnect to a serial port.'); errstr='Serial connection invalid'; return end
case 'open',
% check openness if ~strcmpi(get(ser,'Status'),'open'), disp('Serial connection not opened, please
recreate the object to reconnect to a serial port.'); errstr='Serial connection not opened'; return end
Diseño y construcción de un vehículo autónomo de dos ruedas 124
Honorio Romero Rodríguez
otherwise
% complain error('second argument must be either ''valid'' or
''open''');
end
end % chackser
end % static methods
end % class def
Codigo_Arduino_A3_A4
/* Analog and Digital Input and Output Server for MATLAB
*/
/* This file is meant to be used with the MATLAB arduino IO
package, however, it can be used from the IDE
environment
(or any other serial terminal) by typing commands like:
0e0 : assigns digital pin #4 (e) as input
0f1 : assigns digital pin #5 (f) as output
0n1 : assigns digital pin #13 (n) as output
1c : reads digital pin #2 (c)
1e : reads digital pin #4 (e)
2n0 : sets digital pin #13 (n) low
2n1 : sets digital pin #13 (n) high
2f1 : sets digital pin #5 (f) high
2f0 : sets digital pin #5 (f) low
4j2 : sets digital pin #9 (j) to 50=ascii(2) over 255
4jz : sets digital pin #9 (j) to 122=ascii(z) over 255
3a : reads analog pin #0 (a)
3f : reads analog pin #5 (f)
R0 : sets analog reference to DEFAULT
R1 : sets analog reference to INTERNAL
R2 : sets analog reference to EXTERNAL
99 : returns script type (1 basic, 2 motor, 3 general)
*/
Diseño y construcción de un vehículo autónomo de dos ruedas 125
Honorio Romero Rodríguez
/* define internal for the MEGA as 1.1V (as as for the 328)
*/
#include <Servo.h> // transformacion de señal digital a
analogica
#if defined(__AVR_ATmega1280__) ||
defined(__AVR_ATmega2560__)
#define INTERNAL INTERNAL1V1
#endif
//***********************************
#include <Wire.h>//libreria del puerto i2c
byte angulo[] = 0,0,0,0;//variable a almacenar el angulo
en dos partes, entera y decimal
int inclinacion = 0; //variable que formara el angulo con
sus dos partes, pej 92,45----->pasa a 9245 en la entrada
analogica A3
int yaw = 0; // yaw para analogica A2
Servo futaba ;// variable para nombrar al servo
//mi Ardumu slave sera el 4
//***********************************
void setup()
/* Make sure all pins are put in high impedence state and
that their registers are set as low before doing
anything.
This puts the board in a known (and harmless) state
*/
int i;
for (i=0;i<20;i++)
pinMode(i,INPUT);
digitalWrite(i,0);
/* initialize serial
*/
Serial.begin(115200);
//*********************************************************
*****************************************
Wire.begin();//inicia la comunicacion i2c con los
esclavos,se toma Arduimu como esclavo 4 por ejemplo
pinMode(10,OUTPUT);//configura el pin 19 como salida PWM
futaba.attach(2); // inicio el servo en el pin 2
//*********************************************************
*****************************************
void loop()
/***********************bloque de recepcion de paquete de
4 bytes conteniendo parte entera y decimal del
angulo******************/
Diseño y construcción de un vehículo autónomo de dos ruedas 126
Honorio Romero Rodríguez
/**********************************************************
***********************************************************
*******/
Wire.beginTransmission(4); //inicia la transmision de
Arduimu
Wire.requestFrom(4, 4); // requiere 4 bytes de Arduimu
int j = 0;
while(Wire.available()) //Arduimu puede enviar
angulo[j] = Wire.read(); // receibe un byte
j++;
Wire.endTransmission(4); //finaliza la transmision
/**********************************************************
***********************************************************
************/
/* variables declaration and initialization
*/
static int s = -1; /* state
*/
static int pin = 13; /* generic pin number
*/
int val = 0; /* generic value read from
serial */
int agv = 0; /* generic analog value
*/
int dgv = 0; /* generic digital value
*/
/* The following instruction constantly checks if
anything
is available on the serial port. Nothing gets executed
in
the loop if nothing is available to be read, but as
soon
as anything becomes available, then the part coded
after
the if statement (that is the real stuff) gets
executed */
if (Serial.available() >0)
/* whatever is available from the serial is read here
*/
val = Serial.read();
/* This part basically implements a state machine that
Diseño y construcción de un vehículo autónomo de dos ruedas 127
Honorio Romero Rodríguez
reads the serial port and makes just one transition
to a new state, depending on both the previous state
and the command that is read from the serial port.
Some commands need additional inputs from the serial
port, so they need 2 or 3 state transitions (each
one
happening as soon as anything new is available from
the serial port) to be fully executed. After a
command
is fully executed the state returns to its initial
value s=-1
*/
switch (s)
/* s=-1 means NOTHING RECEIVED YET
******************* */
case -1:
/* calculate next state
*/
if (val>47 && val<90)
/* the first received value indicates the mode
49 is ascii for 1, ... 90 is ascii for Z
s=0 is change-pin mode
s=10 is DI; s=20 is DO; s=30 is AI; s=40 is
AO;
s=90 is query script type (1 basic, 2 motor)
s=340 is change analog reference
*/
s=10*(val-48);
/* the following statements are needed to handle
unexpected first values coming from the serial (if
the value is unrecognized then it defaults to s=-
1) */
if ((s>40 && s<90) || (s>90 && s!=340))
s=-1;
/* the break statements gets out of the switch-case,
so
/* we go back to line 97 and wait for new serial data
*/
break; /* s=-1 (initial state) taken care of
*/
Diseño y construcción de un vehículo autónomo de dos ruedas 128
Honorio Romero Rodríguez
/* s=0 or 1 means CHANGE PIN MODE
*/
case 0:
/* the second received value indicates the pin
from abs('c')=99, pin 2, to abs('t')=116, pin 19
*/
if (val>98 && val<117)
pin=val-97; /* calculate pin
*/
s=1; /* next we will need to get 0 or 1 from serial
*/
else
s=-1; /* if value is not a pin then return to -1
*/
break; /* s=0 taken care of
*/
case 1:
/* the third received value indicates the value 0 or
1 */
if (val>47 && val<50)
/* set pin mode
*/
if (val==48)
pinMode(pin,INPUT);
else
pinMode(pin,OUTPUT);
s=-1; /* we are done with CHANGE PIN so go to -1
*/
break; /* s=1 taken care of
*/
/* s=10 means DIGITAL INPUT
************************** */
case 10:
/* the second received value indicates the pin
from abs('c')=99, pin 2, to abs('t')=116, pin 19
*/
if (val>98 && val<117)
Diseño y construcción de un vehículo autónomo de dos ruedas 129
Honorio Romero Rodríguez
pin=val-97; /* calculate pin
*/
dgv=digitalRead(pin); /* perform Digital Input
*/
Serial.println(dgv); /* send value via serial
*/
s=-1; /* we are done with DI so next state is -1
*/
break; /* s=10 taken care of
*/
/* s=20 or 21 means DIGITAL OUTPUT
******************* */
case 20:
/* the second received value indicates the pin
from abs('c')=99, pin 2, to abs('t')=116, pin 19
*/
if (val>98 && val<117)
pin=val-97; /* calculate pin
*/
s=21; /* next we will need to get 0 or 1 from
serial */
else
s=-1; /* if value is not a pin then return to -1
*/
break; /* s=20 taken care of
*/
case 21:
/* the third received value indicates the value 0 or
1 */
if (val>47 && val<50)
dgv=val-48; /* calculate value
*/
digitalWrite(pin,dgv); /* perform Digital Output
*/
s=-1; /* we are done with DO so next state is -1
*/
break; /* s=21 taken care of
*/
Diseño y construcción de un vehículo autónomo de dos ruedas 130
Honorio Romero Rodríguez
/* s=30 means ANALOG INPUT
*************************** */
case 30:
/* the second received value indicates the pin
from abs('a')=97, pin 0, to abs('f')=102, pin 6,
note that these are the digital pins from 14 to 19
located in the lower right part of the board
*/
pin=val-97; /* calculate pin */
/******************************************************
Modificacion para leer el angulo de memoria capturado de la
IMU */
if (pin>1 && pin<3) /*y almacenarlo
en la salida A2 analogica para cuando seareclamado*/
yaw=((angulo[2])*100+angulo[3]);
agv=yaw;
/*desde
Matlab****************************************************/
Serial.println(agv);
if (pin>2 && pin<4) /*y almacenarlo
en la salida A3 analogica para cuando seareclamado*/
inclinacion=((angulo[0])*100+angulo[1]);
agv=inclinacion;
/*desde
Matlab****************************************************/
Serial.println(agv);
if (val>96 && val<103 && val!=100 && val!=99)
/* se añade justo la de pin analogico A2=
pin3***************************/
pin=val-97; /* calculate pin */
agv=analogRead(pin); /* perform Analog Input
*/
Serial.println(agv); /* send value via serial
*/
s=-1; /* we are done with AI so next state is -1
*/
break; /* s=30 taken care of
*/
Diseño y construcción de un vehículo autónomo de dos ruedas 131
Honorio Romero Rodríguez
/* s=40 or 41 means ANALOG OUTPUT
******************** */
case 40:
/* the second received value indicates the pin
from abs('c')=99, pin 2, to abs('t')=116, pin 19
*/
if (val>98 && val<117)
pin=val-97; /* calculate pin
*/
s=41; /* next we will need to get value from serial
*/
else
s=-1; /* if value is not a pin then return to -1
*/
break; /* s=40 taken care of
*/
case 41:
/* the third received value indicates the analog
value */
analogWrite(pin,val); /* perform Analog Output
*/
s=-1; /* we are done with AO so next state is -1
*/
break; /* s=41 taken care of
*/
/* s=90 means Query Script Type (1 basic, 2 motor)
*/
case 90:
if (val==57)
/* if string sent is 99 send script type via
serial */
Serial.println(1);
s=-1; /* we are done with this so next state is -1
*/
break; /* s=90 taken care of
*/
/* s=340 or 341 means ANALOG REFERENCE
*************** */
Diseño y construcción de un vehículo autónomo de dos ruedas 132
Honorio Romero Rodríguez
case 340:
/* the second received value indicates the reference,
which is encoded as is 0,1,2 for DEFAULT, INTERNAL
and EXTERNAL, respectively
*/
switch (val)
case 48:
analogReference(DEFAULT);
break;
case 49:
analogReference(INTERNAL);
break;
case 50:
analogReference(EXTERNAL);
break;
default: /* unrecognized, no action
*/
break;
s=-1; /* we are done with this so next state is -1
*/
break; /* s=341 taken care of
*/
/* ******* UNRECOGNIZED STATE, go back to s=-1
******* */
default:
/* we should never get here but if we do it means we
are in an unexpected state so whatever is the
second
received value we get out of here and back to s=-1
*/
s=-1; /* go back to the initial state, break
unneeded */
/* end switch on state s
*/
Diseño y construcción de un vehículo autónomo de dos ruedas 133
Honorio Romero Rodríguez
/* end if serial available
*/
/* end loop statement
*/
Arduimu
// Released under Creative Commons License
// Code by Jordi Munoz and William Premerlani, Supported by
Chris Anderson and Doug Weibel
// Version 1.0 for flat board updated by Doug Weibel and
Jose Julio
// Version 1.7 includes support for SCP1000 absolute
pressure sensor
// Version 1.8 uses DIYDrones GPS, FastSerial, and Compass
libraries
// Axis definition: X axis pointing forward, Y axis
pointing to the right and Z axis pointing down.
// Positive pitch : nose up
// Positive roll : right wing down
// Positive yaw : clockwise
#include <avr/eeprom.h>
#include <Wire.h>
#include <FastSerial.h> // ArduPilot Fast Serial
Library
#include <AP_GPS.h> // ArduPilot GPS library
#include <APM_Compass.h> // ArduPilot Mega Magnetometer
Library
//*********************************************************
*************
// This section contains USER PARAMETERS !!!
//
//*********************************************************
*************
Diseño y construcción de un vehículo autónomo de dos ruedas 134
Honorio Romero Rodríguez
// *** NOTE! Hardware version - Can be used for v1
(daughterboards) or v2 (flat)
#define BOARD_VERSION 2 // 1 For V1 and 2 for V2
#define GPS_CONNECTION 0 // 0 for GPS pins, 1 for
programming pins
// GPS Type Selection - Note Ublox or MediaTek is
recommended. Support for NMEA is limited.
#define GPS_PROTOCOL 1 // 1 - NMEA, 2 - EM406, 3 -
Ublox, 4 -- MediaTek
// Enable Air Start uses Remove Before Fly flag -
connection to pin 6 on ArduPilot
#define ENABLE_AIR_START 0 // 1 if using
airstart/groundstart signaling, 0 if not
#define GROUNDSTART_PIN 8 // Pin number used for ground
start signal (recommend 10 on v1 and 8 on v2 hardware)
/*Min Speed Filter for Yaw drift Correction*/
#define SPEEDFILT 2 // >1 use min speed filter for yaw
drift cancellation (m/s), 0=do not use speed filter
/*For debugging propurses*/
#define PRINT_DEBUG 1 //Will print Debug messages
//OUTPUTMODE=1 will print the corrected data, 0 will print
uncorrected data of the gyros (with drift), 2 will print
accelerometer only data
#define OUTPUTMODE 1
#define PRINT_DCM 0 //Will print the whole direction
cosine matrix
#define PRINT_ANALOGS 0 //Will print the analog raw data
#define PRINT_EULER 1 //Will print the Euler angles Roll,
Pitch and Yaw
#define PRINT_GPS 0 //Will print GPS data
// *** NOTE! To use ArduIMU with ArduPilot you must
select binary output messages (change to 1 here)
#define PRINT_BINARY 0 //Will print binary message and
suppress ASCII messages (above)
// *** NOTE! Performance reporting is only supported for
Ublox. Set to 0 for others
#define PERFORMANCE_REPORTING 0 //Will include performance
reports in the binary output ~ 1/2 min
/* Support for optional magnetometer (1 enabled, 0
dissabled) */
Diseño y construcción de un vehículo autónomo de dos ruedas 135
Honorio Romero Rodríguez
#define USE_MAGNETOMETER 0 // use 1 if you want to make yaw
gyro drift corrections using the optional magnetometer
// Local magnetic declination
// I use this web :
http://www.ngdc.noaa.gov/geomagmodels/Declination.jsp
#define MAGNETIC_DECLINATION -1.5 // corrects magnetic
bearing to true north (En Sevilla es -1.5)
/* Support for optional barometer (1 enabled, 0 dissabled)
*/
#define USE_BAROMETER 0 // use 1 if you want to get
altitude using the optional absolute pressure sensor
#define ALT_MIX 50 // For binary messages:
GPS or barometric altitude. 0 to 100 = % of barometric.
For example 75 gives 25% GPS alt and 75% baro
//*********************************************************
*************
// End of user parameters
//*********************************************************
*************
#define SOFTWARE_VER "1.8.1"
// GPS Selection
FastSerialPort0(Serial); // Instantiate the fast
serial driver
#if GPS_PROTOCOL == 1
AP_GPS_NMEA GPS(&Serial);
#elif GPS_PROTOCOL == 2
AP_GPS_406 GPS(&Serial);
#elif GPS_PROTOCOL == 3
AP_GPS_UBLOX GPS(&Serial);
#elif GPS_PROTOCOL == 4
AP_GPS_MTK GPS(&Serial);
#else
# error Must define GPS_PROTOCOL with a valid value.
#endif
// ADC : Voltage reference 3.3v / 10bits(1024 steps) =>
3.22mV/ADC step
// ADXL335 Sensitivity(from datasheet) => 330mV/g,
3.22mV/ADC step => 330/3.22 = 102.48
// Tested value : 101
#define GRAVITY 101 //this equivalent to 1G in the raw data
coming from the accelerometer
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw
data of the accel to actual acceleration in meters for
seconds square
Diseño y construcción de un vehículo autónomo de dos ruedas 136
Honorio Romero Rodríguez
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
// LPR530 & LY530 Sensitivity (from datasheet) =>
3.33mV/º/s, 3.22mV/ADC step => 1.03
// Tested values : 0.96,0.96,0.94
#define Gyro_Gain_X 0.92 //X axis Gyro gain
#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the
scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the
scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the
scaled ADC raw data of the gyro in radians for second
#define Kp_ROLLPITCH 0.015
#define Ki_ROLLPITCH 0.000010
#define Kp_YAW 1.2
//#define Kp_YAW 2.5 //High yaw drift correction gain
- use with caution!
#define Ki_YAW 0.00005
#define ADC_WARM_CYCLES 75
#define FALSE 0
#define TRUE 1
float G_Dt=0.02; // Integration time (DCM algorithm)
long timeNow=0; // Hold the milliseond value for now
long timer=0; //general purpuse timer
long timer_old;
long timer24=0; //Second timer used to print values
boolean groundstartDone = false; // Used to not repeat
ground start
float AN[8]; //array that store the 6 ADC filtered data
float AN_OFFSET[8]; //Array that stores the Offset of the
gyros
float Accel_Vector[3]= 0,0,0; //Store the acceleration in
a vector
float Gyro_Vector[3]= 0,0,0;//Store the gyros rutn rate
in a vector
float Omega_Vector[3]= 0,0,0; //Corrected Gyro_Vector
data
float Omega_P[3]= 0,0,0;//Omega Proportional correction
float Omega_I[3]= 0,0,0;//Omega Integrator
float Omega[3]= 0,0,0;
Diseño y construcción de un vehículo autónomo de dos ruedas 137
Honorio Romero Rodríguez
// Euler angles
int manolito = 34;
float roll; // este es le que a mi m
interesa!!!************************************************
*********
/**********************************************************
****************/
double angulo = 0; //variable intermedia para enviar por
Wire i2c ********ROLL!!******************************
int angulo1 = 0; /* intermedia*/
double angulo2 = 0;
double angulo3 = 0; //variable intermedia para enviar por
Wire i2c ********YAW!!********************************
int angulo4 = 0; /* intermedia*/
double angulo5 = 0;
byte inclinacion[]=0,0,0,0; /*array que almacena partes
enteras y decimales del ROLL y el YAW**************/
int i = 0;
int rollentero = 0;
int rolldecimal = 0;
/**********************************************************
****************/
float pitch;
float yaw;
int toggleMode=0;
float errorRollPitch[3]= 0,0,0;
float errorYaw[3]= 0,0,0;
float errorCourse=180;
float COGX=0; //Course overground X axis
float COGY=1; //Course overground Y axis
unsigned int cycleCount=0;
byte gyro_sat=0;
float DCM_Matrix[3][3]=
1,0,0
,
0,1,0
,
0,0,1
;
float Update_Matrix[3][3]=0,1,2,3,4,5,6,7,8;
//Gyros here
float Temporary_Matrix[3][3]=
0,0,0
Diseño y construcción de un vehículo autónomo de dos ruedas 138
Honorio Romero Rodríguez
,
0,0,0
,
0,0,0
;
// Startup GPS variables
int gps_fix_count = 5; //used to count 5 good fixes
at ground startup
//ADC variables
volatile uint8_t MuxSel=0;
volatile uint8_t analog_reference = DEFAULT;
volatile uint16_t analog_buffer[8];
volatile uint8_t analog_count[8];
#if BOARD_VERSION == 1
uint8_t sensors[6] = 0,2,1,3,5,4; // Use these two
lines for Hardware v1 (w/ daughterboards)
int SENSOR_SIGN[]= 1,-1,1,-1,1,-1,-1,-1,-1; //Sensor:
GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
#endif
#if BOARD_VERSION == 2
uint8_t sensors[6] = 6,7,3,0,1,2; // For Hardware v2
flat
int SENSOR_SIGN[] = 1,-1,-1,1,-1,1,-1,-1,-1;
#endif
// Performance Monitoring variables
// Data collected and reported for ~1/2 minute intervals
#if PERFORMANCE_REPORTING == 1
int mainLoop_count = 0; //Main loop cycles
since last report
int G_Dt_max = 0.0; //Max main loop cycle
time in milliseconds
byte gyro_sat_count = 0;
byte adc_constraints = 0;
byte renorm_sqrt_count = 0;
byte renorm_blowup_count = 0;
byte gps_messages_sent = 0;
long perf_mon_timer = 0;
#endif
unsigned int imu_health = 65012;
//*********************************************************
*************
// This section contains SCP1000_D11 PARAMETERS !!!
//*********************************************************
*************
Diseño y construcción de un vehículo autónomo de dos ruedas 139
Honorio Romero Rodríguez
#if USE_BAROMETER == 1
#define SCP_MODE (9) // 9 = high speed
mode, 10 = high resolution mode
#define PRESSURE_ADDR (0x11U) // IIC address of
the SCP1000
// ************ #define START_ALTITUDE (217U)
// default initial altitude in m above sea level
// When we have to manage data transfers via IIC directly
we need to use the following addresses
// IIC address of the SCP1000 device forms the Top 7 bits
of the address with the R/W bit as the LSB
#define READ_PRESSURE_ADDR (PRESSURE_ADDR<<1 | 1)
#define WRITE_PRESSURE_ADDR (PRESSURE_ADDR<<1)
// SCP1000 Register addresses
#define SNS_ADDR_POPERATION (0x03U) // OPERATON
register
#define SNS_ADDR_PSTATUS (0x07U) // STATUS register
#define SNS_ADDR_PPRESSURE (0x80U) // DATARD16
Register (pressure)
#define SNS_ADDR_DATARD8 (0x7FU) // DAYARD8 Register
#define SNS_ADDR_PTEMP (0x81U) // TEMPOUT
Register (temperature)
#ifndef TRUE
#define TRUE (0x01)
#endif
#ifndef FALSE
#define FALSE (0x00)
#endif
int temp_unfilt = 0;
int temperature = 0;
unsigned long press = 0;
unsigned long press_filt = 0;
unsigned long press_gnd = 0;
long ground_alt = 0; // Ground altitude
in centimeters
long press_alt = 0; // Pressure
altitude in centimeters
#endif
//*********************************************************
********************************
void setup()
Serial.begin(38400, 128, 16);
Diseño y construcción de un vehículo autónomo de dos ruedas 140
Honorio Romero Rodríguez
//******************************I2C************************
****************************************
Wire.begin(4);//asigno el 4 para mi arduIMU
slave**************************************************
Wire.onRequest(I2C);//cuando el maestro
llama******************************************************
//*********************************************************
****************************************
pinMode(2,OUTPUT); //Serial Mux
if (GPS_CONNECTION == 0)
digitalWrite(2,HIGH); //Serial Mux
else
digitalWrite(2,LOW); //Serial Mux
pinMode(5,OUTPUT); //Red LED
pinMode(6,OUTPUT); // Blue LED
pinMode(7,OUTPUT); // Yellow LED
pinMode(GROUNDSTART_PIN,INPUT); // Remove Before Fly
flag (pin 6 on ArduPilot)
digitalWrite(GROUNDSTART_PIN,HIGH);
digitalWrite(5,HIGH);
delay(500);
digitalWrite(6,HIGH);
delay(500);
digitalWrite(7,HIGH);
delay(500);
digitalWrite(5,LOW);
delay(500);
digitalWrite(6,LOW);
delay(500);
digitalWrite(7,LOW);
Analog_Reference(EXTERNAL);//Using external analog
reference
Analog_Init();
debug_print("Welcome...");
#if BOARD_VERSION == 1
debug_print("You are using Hardware Version 1...");
#endif
#if BOARD_VERSION == 2
debug_print("You are using Hardware Version 2...");
#endif
GPS.init(); // GPS Initialization
Diseño y construcción de un vehículo autónomo de dos ruedas 141
Honorio Romero Rodríguez
debug_handler(0); //Printing version
#if USE_MAGNETOMETER == 1
APM_Compass.Init(); // I2C initialization
debug_handler(3);
#endif
// SETUP FOR SCP1000_D11
#if USE_BAROMETER==1
debug_handler(4);
setup_scp();
#endif
if(ENABLE_AIR_START)
debug_handler(1);
startup_air();
else
debug_handler(2);
startup_ground();
delay(250);
Read_adc_raw(); // ADC initialization
timer=DIYmillis();
delay(20);
//*********************************************************
******************************
void loop() //Main Loop
timeNow = millis();
if((timeNow-timer)>=20) // Main loop runs at 50Hz
timer_old = timer;
timer = timeNow;
#if PERFORMANCE_REPORTING == 1
mainLoop_count++;
if (timer-timer_old > G_Dt_max) G_Dt_max = timer-
timer_old;
#endif
Diseño y construcción de un vehículo autónomo de dos ruedas 142
Honorio Romero Rodríguez
G_Dt = (timer-timer_old)/1000.0; // Real time of
loop run. We use this on the DCM algorithm (gyro
integration time)
if(G_Dt > 1)
G_Dt = 0; //Something is wrong - keeps dt from
blowing up, goes to zero to keep gyros from departing
// *** DCM algorithm
Read_adc_raw();
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
#if PRINT_BINARY == 1
printdata(); //Send info via serial
#endif
//Turn on the LED when you saturate any of the gyros.
if((abs(Gyro_Vector[0])>=ToRad(300))||(abs(Gyro_Vector[1])>
=ToRad(300))||(abs(Gyro_Vector[2])>=ToRad(300)))
gyro_sat=1;
#if PERFORMANCE_REPORTING == 1
gyro_sat_count++;
#endif
digitalWrite(5,HIGH);
cycleCount++;
// Do these things every 6th time through the main
cycle
// This section gets called every 1000/(20*6) = 8
1/3 Hz
// doing it this way removes the need for another
'millis()' call
// and balances the processing load across main
loop cycles.
switch (cycleCount)
case(0):
GPS.update();
break;
case(1):
Diseño y construcción de un vehículo autónomo de dos ruedas 143
Honorio Romero Rodríguez
//Here we will check if we are getting
a signal to ground start
if(digitalRead(GROUNDSTART_PIN) == LOW
&& groundstartDone == false)
startup_ground();
break;
case(2):
#if USE_BAROMETER==1
ReadSCP1000(); // Read I2C
absolute pressure sensor
press_filt = (press + 2l *
press_filt) / 3l; //Light filtering
//temperature = (temperature * 9 +
temp_unfilt) / 10; We will just use the ground temp for
the altitude calculation
#endif
break;
case(3):
#if USE_MAGNETOMETER==1
APM_Compass.Read(); // Read
magnetometer
APM_Compass.Calculate(roll,pitch); //
Calculate heading
#endif
break;
case(4):
// Display Status on LEDs
// GYRO Saturation indication
if(gyro_sat>=1)
digitalWrite(5,HIGH); //Turn Red
LED when gyro is saturated.
if(gyro_sat>=8) // keep the LED
on for 8/10ths of a second
gyro_sat=0;
else
gyro_sat++;
else
digitalWrite(5,LOW);
// YAW drift correction indication
if(GPS.ground_speed<SPEEDFILT*100)
digitalWrite(7,HIGH); // Turn
on yellow LED if speed too slow and yaw correction
supressed
else
digitalWrite(7,LOW);
Diseño y construcción de un vehículo autónomo de dos ruedas 144
Honorio Romero Rodríguez
// GPS Fix indication
switch (GPS.status())
case(2):
digitalWrite(6,HIGH);
//Turn Blue LED when gps is fixed.
break;
case(1):
if
(GPS.valid_read == true)
toggleMode = abs(toggleMode-1); // Toggle blue light on and
off to indicate NMEA sentences exist, but no GPS fix lock
if
(toggleMode==0)
digitalWrite(6, LOW); // Blue light off
else
digitalWrite(6, HIGH); // Blue light on
GPS.valid_read = false;
break;
default:
digitalWrite(6,LOW);
break;
break;
case(5):
cycleCount = -1; // Reset case
counter, will be incremented to zero before switch
statement
#if !PRINT_BINARY
printdata(); //Send info via
serial
#endif
break;
#if PERFORMANCE_REPORTING == 1
if (timeNow-perf_mon_timer > 20000)
printPerfData(timeNow-perf_mon_timer);
Diseño y construcción de un vehículo autónomo de dos ruedas 145
Honorio Romero Rodríguez
perf_mon_timer=timeNow;
#endif
//*********************************************************
***********************
void startup_ground(void)
uint16_t store=0;
int flashcount = 0;
debug_handler(2);
for(int c=0; c<ADC_WARM_CYCLES; c++)
digitalWrite(7,LOW);
digitalWrite(6,HIGH);
digitalWrite(5,LOW);
delay(50);
Read_adc_raw();
digitalWrite(7,HIGH);
digitalWrite(6,LOW);
digitalWrite(5,HIGH);
delay(50);
Read_adc_raw();
delay(20);
Read_adc_raw();
for(int y=0; y<=5; y++) // Read first initial ADC
values for offset.
AN_OFFSET[y]=AN[y];
#if USE_BAROMETER==1
ReadSCP1000();
press_gnd = press;
temperature = temp_unfilt;
delay(20);
#endif
for(int i=0;i<400;i++) // We take some readings...
Read_adc_raw();
for(int y=0; y<=5; y++) // Read initial ADC
values for offset (averaging).
AN_OFFSET[y]=AN_OFFSET[y]*0.8 + AN[y]*0.2;
delay(20);
if(flashcount == 5)
Diseño y construcción de un vehículo autónomo de dos ruedas 146
Honorio Romero Rodríguez
digitalWrite(7,LOW);
digitalWrite(6,HIGH);
digitalWrite(5,LOW);
if(flashcount >= 10)
flashcount = 0;
#if USE_BAROMETER==1
ReadSCP1000();
press_gnd = (press_gnd * 9l + press) / 10l;
temperature = (temperature * 9 +
temp_unfilt) / 10;
#endif
digitalWrite(7,HIGH);
digitalWrite(6,LOW);
digitalWrite(5,HIGH);
flashcount++;
digitalWrite(5,LOW);
digitalWrite(6,LOW);
digitalWrite(7,LOW);
AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
for(int y=0; y<=5; y++)
Serial.println(AN_OFFSET[y]);
store = ((AN_OFFSET[y]-200.f)*100.0f);
eeprom_busy_wait();
eeprom_write_word((uint16_t *) (y*2+2),
store);
while (gps_fix_count > 0 && USE_BAROMETER)
GPS.update();
// Serial.print(gpsFix);
// Serial.print(", ");
// Serial.println(gpsFixnew);
if (GPS.fix == 1 && GPS.new_data == 1)
GPS.new_data = 0;
gps_fix_count--;
#if USE_BAROMETER==1
press_filt = press_gnd;
ground_alt = GPS.altitude;
eeprom_busy_wait();
eeprom_write_dword((uint32_t *)0x10, press_gnd);
eeprom_busy_wait();
Diseño y construcción de un vehículo autónomo de dos ruedas 147
Honorio Romero Rodríguez
eeprom_write_word((uint16_t *)0x14, temperature);
eeprom_busy_wait();
eeprom_write_word((uint16_t *)0x16, (ground_alt/100));
#endif
groundstartDone = true;
debug_handler(6);
//******************************************************I2C
***********************************************************
******
void I2C()
angulo=ToDeg(roll)+90; /*con esto obtengo
la parte entera en rollentero y la parte*/
angulo1=angulo*1; /*decimal en
rolldecimal************************************/
angulo2=angulo-angulo1;
inclinacion[0] =(byte)((angulo)); /****aqui tengo la
parte entera****************************/
inclinacion[1]=(byte)((angulo2)*100); /****aqui la parte
decimal*********************************/
angulo3=ToDeg(yaw)+90;
angulo4=(angulo3)*1;
angulo5=angulo3-angulo4;
inclinacion[2] =(byte)((angulo3));
inclinacion[3] =(byte)((angulo5)*100);
Wire.send(inclinacion,4); /***envio paquete
de 4 bytes a gestionar por el maestro Arduino****/
/**conteniendo las
partes enteras y decimales del ROLL y el YAW***/
//*********************************************************
***********************************************************
*******
void startup_air(void)
Diseño y construcción de un vehículo autónomo de dos ruedas 148
Honorio Romero Rodríguez
uint16_t temp=0;
for(int y=0; y<=5; y++)
eeprom_busy_wait();
temp = eeprom_read_word((uint16_t *) (y*2+2));
AN_OFFSET[y] = temp/100.f+200.f;
Serial.println(AN_OFFSET[y]);
#if USE_BAROMETER==1
eeprom_busy_wait();
press_gnd = eeprom_read_dword((uint32_t *) 0x10);
press_filt = press_gnd;
eeprom_busy_wait();
temperature = eeprom_read_word((uint16_t *) 0x14);
eeprom_busy_wait();
ground_alt = eeprom_read_word((uint16_t *) 0x16);
ground_alt *= 100;
#endif
Serial.println("***Air Start complete");
void debug_print(char string[])
#if PRINT_DEBUG != 0
Serial.print("???");
Serial.print(string);
Serial.println("***");
#endif
void debug_handler(byte message)
#if PRINT_DEBUG != 0
static unsigned long BAD_Checksum=0;
switch(message)
case 0:
Serial.print("???Software Version ");
Serial.print(SOFTWARE_VER);
Serial.println("***");
break;
case 1:
Serial.println("???Air Start!***");
break;
Diseño y construcción de un vehículo autónomo de dos ruedas 149
Honorio Romero Rodríguez
case 2:
Serial.println("???Ground Start!***");
break;
case 3:
Serial.println("???Enabling Magneto...***");
break;
case 4:
Serial.println("???Enabling Pressure
Altitude...***");
break;
case 5:
Serial.println("???Air Start complete");
break;
case 6:
Serial.println("???Ground Start complete");
break;
case 10:
BAD_Checksum++;
Serial.print("???GPS Bad Checksum: ");
Serial.print(BAD_Checksum);
Serial.println("...***");
break;
default:
Serial.println("???Invalid debug ID...***");
break;
#endif
/*
EEPROM memory map
0 0x00 Unused
1 0x01 ..
2 0x02 AN_OFFSET[0]
3 0x03 ..
4 0x04 AN_OFFSET[1]
5 0x05 ..
6 0x06 AN_OFFSET[2]
7 0x07 ..
8 0x08 AN_OFFSET[3]
9 0x09 ..
10 0x0A AN_OFFSET[4]
Diseño y construcción de un vehículo autónomo de dos ruedas 150
Honorio Romero Rodríguez
11 0x0B ..
12 0x0C AN_OFFSET[5]
13 0x0D ..
14 0x0E Unused
15 0x0F ..
16 0x10 Ground Pressure
17 0x11 ..
18 0x12 ..
19 0x13 ..
20 0x14 Ground Temp
21 0x15 ..
22 0x16 Ground Altitude
23 0x17 ..
*/
Diseño y construcción de un vehículo autónomo de dos ruedas 151
Honorio Romero Rodríguez
8.2. Datasheets
Transistores puente H, TIP142 y TIP147
Diseño y construcción de un vehículo autónomo de dos ruedas 152
Honorio Romero Rodríguez
Transistor 2N3055:
Diseño y construcción de un vehículo autónomo de dos ruedas 153
Honorio Romero Rodríguez
Transistor BD140 para la etapa intermedia del control de tracción:
Diseño y construcción de un vehículo autónomo de dos ruedas 154
Honorio Romero Rodríguez
Transistores de señal:
Diseño y construcción de un vehículo autónomo de dos ruedas 156
Honorio Romero Rodríguez
8.3. Herramientas
Soldadora MIG en atmósfera de oxígeno.
Diseño y construcción de un vehículo autónomo de dos ruedas 157
Honorio Romero Rodríguez
Piedra desbastadora.
Diseño y construcción de un vehículo autónomo de dos ruedas 158
Honorio Romero Rodríguez
Materiales para el PCB.
Diseño y construcción de un vehículo autónomo de dos ruedas 159
Honorio Romero Rodríguez
8.4. Fotolitos
Fotolito PCB