Treball Final de Graudeeea.urv.cat/public/PROPOSTES/pub/pdf/2420pub.pdf · Treball Final de Grau...
115
Xavier Avila Busquets Construcció d´un robot mòbil de tracció diferencial controlat per Matlab TREBALL DE FI DE GRAU dirigit per Albert Oller Pujol Grau en Enginyeria Electrónica Industrial i Automàtica Tarragona 2016
Transcript of Treball Final de Graudeeea.urv.cat/public/PROPOSTES/pub/pdf/2420pub.pdf · Treball Final de Grau...
Xavier Avila Busquets
Construcció d´un robot mòbil de tracció diferencial controlat per Matlab
TREBALL DE FI DE GRAU
dirigit per Albert Oller Pujol
Grau en Enginyeria Electrónica Industrial i Automàtica
Tarragona
2016
Treball Final de Grau Xavier Avila Busquets
1
Agrair a la meva germana que sense la seva paciència i saviesa no hagués acabat mai la
carrera, als meus pares per sempre estar al meu costat aguantant-me i suportant-me i
finalment a Dídac Coll per la seva ajuda i amistat.
Treball Final de Grau Xavier Avila Busquets
2
Índex general
Índex de figures ..................................................................................................................... 5
Índex de taules ....................................................................................................................... 7
1 Introducció .......................................................................................................................... 9
2 Objectiu ............................................................................................................................ 11
3 Especificacions dels dispositius i Software utilitzats ....................................................... 12
3.1 Característiques d ´Arduino UNO ........................................................................ 13
3.2 Característiques de L298N .................................................................................... 15
3.3 Característiques motor de continua ....................................................................... 16
3.4 JY-MCU HC-06 Bluetooth ................................................................................... 17
3.5 Matlab ................................................................................................................... 18
3.6 Arduino IDE ......................................................................................................... 18
3.6.1 Connexions entre Matlab i Arduino .............................................................. 19
3.7 FreeCAD ............................................................................................................... 20
3.8 GitHub .................................................................................................................. 20
4 Disseny del robot .............................................................................................................. 21
4.1 Consideracions prèvies ......................................................................................... 21
4.2 Mecanisme de propulsió ....................................................................................... 23
4.2.1 Diferents tipus de motors ............................................................................... 23
4.2.2 Control dels motors ....................................................................................... 25
4.3 Estudis del motor .................................................................................................. 29
4.3.1 Programa d ´Arduino ..................................................................................... 30
4.3.2 Anàlisis de les No linealitats del motor ......................................................... 32
4.3.3 Estudi de l´ample de banda ............................................................................ 35
4.3.1 Polinomi de la velocitat ................................................................................. 39
4.3.2 Estimació de paràmetres del motor ............................................................... 40
4.4 Sistema de control PID ......................................................................................... 43
4.4.1 PID en el Arduino .......................................................................................... 44
4.4.2 Obtenció dels valors del PID ......................................................................... 46
4.5 Sistema de comunicació ........................................................................................ 47
4.5.1 Configuració del mòdul Bluetooth ................................................................ 48
4.5.2 Comunicació entre Arduino i Matlab ............................................................ 49
5 Construcció del robot ....................................................................................................... 50
5.1 Primers dissenys ................................................................................................... 50
Treball Final de Grau Xavier Avila Busquets
3
5.1.1 Disseny del xassís principal ........................................................................... 50
5.1.2 Disseny dels engranatges ............................................................................... 50
5.2 Mètode d’impressió de les peces .......................................................................... 52
5.3 Muntatge de les peces ........................................................................................... 54
5.4 Pressupost ............................................................................................................. 57
6 Problemes i solucions ....................................................................................................... 58
6.1 Errors en comunicació .......................................................................................... 59
6.2 Impressió de les peces ........................................................................................... 60
6.3 Balanç de càrregues .............................................................................................. 61
6.4 Creació de plànols ................................................................................................. 62
7 Programa de control del robot .......................................................................................... 63
7.1 Matlab GUI ........................................................................................................... 63
7.2 Interfície amb el usuari ......................................................................................... 65
7.3 Android ................................................................................................................. 67
8 Conclusions finals del projecte ......................................................................................... 69
9 Webgrafia ......................................................................................................................... 70
10 Annexes .......................................................................................................................... 71
10.1 Programes d ‘Arduino ........................................................................................... 71
10.1.1 Estudi del motor ............................................................................................ 71
10.1.2 Control PID ................................................................................................... 76
10.1.3 Codi configuració Bluetooth ......................................................................... 84
10.1.4 Codi per a linealització de Velocitat.............................................................. 84
10.1.5 Codi per a trobar els valors del motor ........................................................... 87
10.2 Programes de Matlab ............................................................................................ 90
10.2.1 Estudis del motor ........................................................................................... 90
10.3 Comandes AT per Bluetooth HC-06 .................................................................... 99
10.4 Programa de control del robot ............................................................................ 100
10.4.1 Programa principal de la GUI ...................................................................... 100
10.4.2 Inicialització del port ................................................................................... 102
10.4.3 Funció per al control del motor ................................................................... 103
10.4.4 Funció per la lectura del teclat ..................................................................... 103
10.4.5 Impressió de la gràfica ................................................................................. 104
10.4.6 Funció per calcular els límits de la zona d´operació del robot .................... 105
10.5 Plànols ................................................................................................................. 106
10.5.1 Plànol engranatge roda ................................................................................ 106
Treball Final de Grau Xavier Avila Busquets
4
10.5.3 Plànol engranatge motor .............................................................................. 107
10.5.5 Plànol roda ................................................................................................... 108
10.5.7 Plànol suport de motor ................................................................................. 109
10.5.9 Plànol xassís superior .................................................................................. 110
10.5.12 Plànol xassís ............................................................................................. 112
Treball Final de Grau Xavier Avila Busquets
5
Índex de figures
FIGURA 1.1 LOGOTIP ARDUINO ......................................................................................................................... 9 FIGURA 1.2 LOGOTIP MATLAB ........................................................................................................................ 10 FIGURA 1.3 IMPRESSORA 3D ........................................................................................................................... 10 FIGURA 3.1 CARA DAVANT DE L ´ARDUINO UNO ........................................................................................... 13 FIGURA 3.2 CARA DARRERA DE L ‘ARDUINO UNO ......................................................................................... 13 FIGURA 3.3 CAPTURA DE PANTALLA DE INICIALITZACIÓ DE ARDUINO IDE .................................................... 14 FIGURA 3.4 CAPTURA DE PANTALLA INICIALITZACIÓ DE MATLAB .................................................................. 14 FIGURA 3.5 CARA DAVANTERA DEL DRIVER L298N ....................................................................................... 15 FIGURA 3.6 MOTOR DE CONTINUA AMB ENCODER ........................................................................................... 16 FIGURA 3.7 CARA DE DAVANT DE JY-MCU .................................................................................................... 17 FIGURA 3.8 CARA DE DARRERA DE JY-MCU .................................................................................................. 17 FIGURA 3.9 LOGOTIP DE MATLAB ................................................................................................................... 18 FIGURA 3.10 CAPTURA DE PANTALLA D’ ARDUINO IDE ................................................................................. 18 FIGURA 3.11 CAPTURA DE PANTALLA DELS ARXIUS DESCARREGATS. ............................................................. 19 FIGURA 3.12 LOGOTIP DE FREECAD ............................................................................................................... 20 FIGURA 3.13 LOGOTIP DE GITHUB .................................................................................................................. 20 FIGURA 4.1 FLUXGRAMA DELS CRITERIS PER EL DESENVOLUPAMENT DEL ROBOT. ......................................... 21 FIGURA 4.2 DISSENY FINAL DEL ROBOT ........................................................................................................... 22 FIGURA 4.3 ROBOT “RENACUAJO” DE THINGIVERSE ....................................................................................... 22 FIGURA 4.4 SERVOMOTOR ............................................................................................................................... 23 FIGURA 4.5 MOTOR CC ................................................................................................................................... 24 FIGURA 4.6 DETALL DRIVER L298N ............................................................................................................... 25 FIGURA 4.7 CONFIGURACIÓ DE UN PONT COMPLET .......................................................................................... 25 FIGURA 4.8 FUNCIONAMENT DE ENCODER ABSOLUT ....................................................................................... 27 FIGURA 4.9 FUNCIONAMENT DE ENCODER INCREMENTAL ............................................................................... 28 FIGURA 4.10 CAPTURA DE PANTALLA DE ARDUINO IDE PER A COMUNICACIÓ AMB MATLAB ......................... 30 FIGURA 4.11 GRÀFICA D´ENTRADA DE SENYAL PER A ESTUDI DE NO-LINEALITATS ........................................ 32 FIGURA 4.12 CAPTURA DE PANTALLA DE LES FUNCIONS CREADES EN MATLAB .............................................. 32 FIGURA 4.13 GRÀFICA D´ESTUDI DE NO-LINEALITATS .................................................................................... 34 FIGURA 4.14 SENYLA D´EXITACIÓ SINUSOÏDAL DE FREQÜÈNCIA VARIABLE .................................................... 35 FIGURA 4.15 ENTRADA I SORTIDA DEL SISTEMA PER A AMPLE DE BANDA ....................................................... 36 FIGURA 4.16 GRÀFICA DE LA FUNCIO DE FOURIER DISCRETA ......................................................................... 37 FIGURA 4.17 GRÀFICA DE BODE ...................................................................................................................... 37 FIGURA 4.18 GRÀFICA DE LINEALITZACIÓ DE LA VELOCITAT .......................................................................... 39 FIGURA 4.19 CIRCUIT ELÈCTRIC EQUIVALENT DE MOTOR DE CONTINUA ......................................................... 40 FIGURA 4.20 GRÀFICA D´ESTIMACIÓ DE PARAMETRES .................................................................................... 42 FIGURA 4.21 DIAGRAMA DE BLOCS D´UN CONTROL PID ................................................................................. 43 FIGURA 4.22 LOGOTIP I DIBUIX DE COMUNICACIÓ BLUETOOTH ...................................................................... 47 FIGURA 4.24 CONNEXIO ENTRE BLUETOOTH I ARDUINO UNO ....................................................................... 48 FIGURA 4.25 TRAMA D´ENVIAMENT A ARDUINO DESDE MATLAB ................................................................... 49 FIGURA 4.26 PRIMER DISSENY DEL XASIS DEL ROBOT ..................................................................................... 50 FIGURA 4.27 ENGRANATGE DEL MOTOR .......................................................................................................... 50 FIGURA 4.28 ENGRANATGE DE RODA .............................................................................................................. 50 FIGURA 5.1 IMPRESSIÓ PER DEPOSICIÓ DE MATERIAL FOS................................................................................ 52 FIGURA 5.2 PROCÉS D´IMPRESSIÓ DE LES PECES .............................................................................................. 53 FIGURA 5.3 FINAL DEL PROCÉS D´IMPRESSIÓ DE LES PECES ............................................................................. 53 FIGURA 5.4 ENSAMBLATGE DEL DRIVER L298N ............................................................................................. 54 FIGURA 5.5 ENSAMBLATGE DE ENGRANATGE AMB RODAMENT ....................................................................... 54 FIGURA 5.6 ENSAMBLATGE MOTORS AMB SUPORTS ........................................................................................ 55 FIGURA 5.7 ENSAMBLATGE MOTOR I ENGRANATGE ........................................................................................ 55 FIGURA 5.8 ENSAMBLATEGE DE RODES I CANICA ............................................................................................ 55 FIGURA 5.9 ENSAAMBLATGE DEL XASSÍS SUPERIOR ........................................................................................ 55 FIGURA 5.10 MONTATGE FINAL DEL ROBOT EN FREECAD ............................................................................... 56
Treball Final de Grau Xavier Avila Busquets
6
FIGURA 9.1 MÓDUL WIFI ESP-01 ESP8266 ................................................................................................... 59 FIGURA 6.1 CAPTURA DE PANTALLA DE LA INTERFÍCIE GRÀFICA EN CONSTRUCCIÓ ....................................... 65 FIGURA 6.2 CAPTURA DE PANTALLA DE GUI CONECTAT ................................................................................. 66 FIGURA 6.3 CAPTURA DE PANTALLA DE GUI DESCONECTAT ........................................................................... 66 FIGURA 6.4 CAPTURA DE PANTALLA DE GUI AMB CONTROL PER BOTONS ...................................................... 66 FIGURA 6.5 CAPTURA DE PANTALLA DE GUI AMB CONTROL PER LLISCADOR ................................................. 66 FIGURA 6.6 CAPTURA DE PANTALLA DE GUI AMB CONTROL PER RATOLÍ........................................................ 66 FIGURA 6.7 CAPTURA DE PANTALLA DE GUI AMB CONTROL PER TECLAT ....................................................... 66 FIGURA 6.8 CAPTURA DE PANTALLA DE SAMSUN S3 MINI DE SELECCIÓ DE CONTROL .................................... 67 FIGURA 6.9 CAPTURA DE PANTALLA DE SAMSUN S3 MINI DEL INICI DE L ÁPLICACIÓ ..................................... 67 FIGURA 6.10 CAPTURA DE PANTALLA DE SAMSUN S3 MINI PER A CONTROL TÀCTIL....................................... 68 FIGURA 6.11 CAPTURA DE PANTALLA DE SAMSUN S3 MINI DE CONNEXIÓ BLUETOOTH PER A CONTROL TÀCTIL
................................................................................................................................................................ 68 FIGURA 6.12 CAPTURA DE PANTALLA DE SAMSUN S3 MINI PER A CONTROL AMB ACCELEROMETRE .............. 68 FIGURA 6.13 CAPTURA DE PANTALLA DE SAMSUN S3 MINI DE CONNEXIÓ BLUETOOTH PER A CONTROL AMB
ACCELEROMETRE..................................................................................................................................... 68
Treball Final de Grau Xavier Avila Busquets
7
Índex de taules
TAULA 3.1 CARACTERÍSTIQUES TÈCNIQUES ARDUINO UNO ........................................................................... 13 TAULA 3.2CARACTERISTIQUES TÈCNIQUES DRIVER L298N ............................................................................. 15 TAULA 3.3 CARACTERÍSTIQUES TÈCNIQUES MOTOR DE CONTINUA ................................................................. 16 TAULA 3.4 CARACTERÍSTIQUES TÈCNIQUES JY-MCU HC-06 BLUETOOTH ..................................................... 17 TAULA 4.1 RESUM DE LES NO-LINEALITATS ................................................................................................... 34 TAULA 5.1 TAULA DE PRESSUPOST .................................................................................................................. 57
Treball Final de Grau Xavier Avila Busquets
8
Treball Final de Grau Xavier Avila Busquets
9
1 Introducció En un món globalitzat com el actual la tecnologia envaeix les nostres cases, llocs de feina i
oci a passes agegantades. Això ha esdevingut en un canvi en les nostres tasques quotidianes
i la forma en que ens comuniquem. També a permès compartir els coneixements de forma
oberta i sense ànims de lucre.
Dins d´aquests avenços hem pogut descobrir una forma senzilla de crear circuits electrònics
amb microcontroladors mitjançant les plaques Arduino.
Arduino es va iniciar el any 2006 com un projecte entre estudiants del institut d’Ivrea (Itàlia)
on un dels seus fundadors, Massimo Banzi, donava classe. També en va ser partícip el
estudiant Colombià Hernando Barragán el qual va desarrelar la targeta electrònica Wiring el
llenguatge de programació i la plataforma. Una vegada confluïda la plataforma els
investigadors van treballar per a evolucionar el producte per a fer-lo més lleuger, econòmic
i disponible per a la comunitat de codi obert. Posteriorment Google va col·laborar en
desenvolupar el Kit Android ADK, una placa Arduino la qual era capaç de comunicar-se
amb telèfons mòbils mitjançant Android. Finalment es va passar a la producció en seria de
la primera versió sota 3 requisits, el cost no fos major de 30€, fos acoblat en una placa blava,
tenia que ser Pug and Play i es pogués treballar amb diferents plataformes informàtiques
com MacOSX, Windows y GNU/Linux.
Una de les altres plataformes utilitzades en ingeriria és Matlab. Matlab és un entorn de
computació numèrica i un llenguatge de programació. MATLAB permet manipular
fàcilment matrius, dibuixar funcions i dades, implementar algorismes, crear interfícies
d'usuari, i comunicar-se amb altres programes en altres llenguatges.
Figura 1.1 Logotip Arduino
Extret de: https://twitter.com/arduino
Treball Final de Grau Xavier Avila Busquets
10
El nom de MATLAB prové de “MAtrix LABoratory”. Desenvolupat per Cleve Moler a
finals dels anys 70, dissenyat per a donar accés als seus estudiants a LINPACK i EISPACK
sense tindre que aprendre Fortran. Jack Little durant una visita a la Universitat de Stanford
al 1983 va veure un gran potencial en ell. Van reescriure´l per a llenguatge C i van fundar
Mathworks el 1984.
Un altre de les innovacions esdevingudes en els últims temps ha estat la possibilitat de
dissenyar i fabricar peces en 3 dimensions mitjançant les impressores 3D les quals mitjançant
la extrusió de plàstic fos i una tècnica anomenada fabricació additiva, és capaç de produir
representacions 3D físiques de models creats per ordinador.
Existeixen múltiples tipus com sinterització làser, estereolitografia, compactació o la més
coneguda per la injecció de polímers. Actualment son utilitzades en tot tipus d’àmbits com
la medicina, per a la creació de pròtesis, automoció o la industria en general.
Figura 1.2 Logotip Matlab
Extret de:
http://www.unibe.edu.do/sites/default/f
iles/taller_de_matlab.png
Figura 1.3 Impressora 3D
Extret de:
https://static.betazeta.com/www.fayerwayer.com/up/2014/10/ar11111
1-960x623.jpg
Treball Final de Grau Xavier Avila Busquets
11
2 Objectiu L´objectiu d’ aquest treball de fi de grau és la creació integra de un robot amb motors de
continua controlat per llaç tancat mitjançant diferents dispositius de forma Wireless. Per tant,
fent ús de Freecad, Matlab i Arduino es realitzarà el control del robot, també es realitzaran
altres aplicacions mitjançant Android. D´aquesta manera és te una idea de l´abast d´aquest
projecte i la varietat d’àmbits utilitzats per a dur ´l a terme. A més es realitzarà un anàlisis i
estudi dels motors de continua.
La intenció final d’aquest treball és crear un robot i el seu control mitjançant Matlab i
Arduino.
Finalment, en cas d’aconseguir l´objectiu principal, es a dir, la creació d´un robot amb
motors de continua i control de llaç tancat, es realitzarà una breu conclusió i comparació de
l’experiència i l´ús d´aquest. D´aquest manera podrem observar la realització d´un prototip
mitjançant eines OpenSource.
Treball Final de Grau Xavier Avila Busquets
12
3 Especificacions dels dispositius i Software utilitzats L´objectiu d´aquest apartat és descriure tots els dispositius que s´han utilitzat per dur a terme
amb èxit l´abast d´aquest treball. Així doncs es detallarà el Hardware que contenen els
dispositius d´entrada de dades, és a dir, Matlab GUI; i els actuadors que s’utilitzaran,
Arduino UNO. A més a més s´explicarà el software necessari i els passos a seguir per a dur
a terme la seva instal·lació per tal d´utilitzar-los amb el programa Matlab.
Treball Final de Grau Xavier Avila Busquets
13
3.1 Característiques d ´Arduino UNO
Arduino és una placa de circuits imprès simple basada en el microcontrolador de codi obert
provinent de la plataforma de codi obert Wiring amb l´objectiu de fer més simple i accessible
el disseny de circuits electrònics amb microcontroladors.
Aquest sistema proporciona conjunts de pins analògics i digitals d’entrada i sortida (I/O) que
pot interactuar amb diverses targetes de comunicació en sèrie, incloent Universal Serial Bus
(USB) en alguns models, per a la càrrega de programes de computadores personals.
A diferencia d´altres microcontroladors, aquest és reprogramada mitjançant un “Bootloader”
o gestor d´arrancada, el qual ens evitar utilitzar un programador extern i utilitzar el nostre
ordinador per a reprogramar-la. A continuació descriurem el Hardware i el Software que
utilitzarem.
Les especificacions tècniques de la placa Arduino UNO son:
Especificacions
- Microcontrolador ATmega328P
- Voltatge operatiu 5 Volts
- Voltatge d´entrada (recomanat) 7-12 Volts
- Voltatge d’entrada (límit) 6-20 Volts
- Pins d’entrades/sortida digital 14 (6 proporcionen PWM)
- Pins d’entrada analògics 6
- DC pels pins d´entrada/sortida 40 mA
- DC pels pins de 3,3 Volts 50 mA
- Memòria Flash 16KB (2KB pel Bootloader)
- SRAM 1KB
- EEPROM 512 bytes
- Velocitat de rellotge 16 Mhz
- Llargada 68,6 mm
- Amplitud 53,4 mm
- Pes 25 g Taula 3.1 Característiques tècniques Arduino UNO
Figura 3.1 Cara davant de l ´Arduino UNO
Extret de: http://store-usa.arduino.cc/products/
Figura 3.2 Cara darrera de l ‘Arduino UNO
Extret de: http://store-usa.arduino.cc/products/
Treball Final de Grau Xavier Avila Busquets
14
El software utilitzat ha sigut:
- Arduino 1.6.6
- Matlab 2014a
Figura 3.3 Captura de pantalla de
inicialització de Arduino IDE
Figura 3.4 Captura de
pantalla inicialització
de Matlab
Treball Final de Grau Xavier Avila Busquets
15
3.2 Característiques de L298N
El driver L298N és un mòdul basat en un chip L298 que et permet controlar dos motors de
corrent continua o un motor pas a pas bipolar de fins a dos ampers.
El mòdul compte amb dos components necessaris per a funcionar sense necessitat
d´elements addicionals, entre ells els díodes de protecció i un regulador LM7805 que
subministra 5 Volts a la part lògica del integrat L298N.
El L298 és un circuit integrat monolític d´alta tensió i alta corrent amb pont complet doble
dissenyat per acceptar nivells lògics TTL i gestionar càrregues inductives com relés,
solenoides, motors de continua i pas a pas. Dues entrades d´habilitació, “ENA” i “ENB”, les
quals activen o desactiven els dispositius independentment dels senyals d´entrada. Els
emissors dels transistors inferiors de cada pont estan connectats entre si. Es proporciona una
entrada d’ alimentació addicional perquè la lògica funciona a un voltatge més baix.
Les especifacions tècniques del driver L298N són:
Especificacions
- Alimentació lògica 6-12 Volts
- Rang d´alimentació 4.8-46 Volts
- Corrent màxima 2A
- Pins d’entrades digitals 6
- Pins de sortida Per a 2 motors DC o un pas a pas
- Temperatura de funcionament -25 a 130 ºC
- Llargada 60 mm
- Amplitud 55 mm
- Altura 26
- Pes 35 g Taula 3.2Caracteristiques tècniques driver L298N
Figura 3.5 Cara davantera del Driver
L298N
Extret de: http://www.dx.com/
Treball Final de Grau Xavier Avila Busquets
16
3.3 Característiques motor de continua
Per a la realització de un llaç tancat és necessari que el motor posseeixi un codificador a més
de una gran resolució. Aquest serà el motor de continua seleccionat degut a la relació qualitat
preu, degut a que el projecte ha estat totalment finançat per a mi mateix.
Proveït de una reductora de 1:34 i un codificador de 11 cicles per volta ens permet tindre
una resolució de 340 cicles per volta, el qual ens permet un millor control d’aquest.
Les especificacionstècniques del motor són:
Especificacions
- Alimentació 6 Volts
- Velocitat en buit 210 rpm
- Diàmetre del eix 4 mm
- Allargada del eix 12 mm
- Senyals del codificador 11 per volta
- Allargada 75 mm
- Diàmetre 25 mm
- Pes 96 g Taula 3.3 Característiques tècniques Motor de continua
Figura 3.6 Motor de continua amb encoder
Extret de http://www.dx.com/p/high-torque-25mm-dc-6-
0v-210rpm-encoder-precision-gear-motor-
415115#.V8BchvmLSM8
Treball Final de Grau Xavier Avila Busquets
17
3.4 JY-MCU HC-06 Bluetooth
Per a la comunicació Wireless s´opta per a la comunicació Bluetooth. El mòdul JY-MCU és
molt indicat per a aquest fi, donat que és una dels més utilitzats i amb el qual es pot trobar
molt de material didàctic per a la utilització d´aquest.
El model HC-06 disposa 4 pins, el qual ens limita en funcionalitat el qual només pot
funcionar com a esclau. Això fa que tingui menys instruccions AT amb les que podrem
treballar que altres dispositius similars.
Les especifacions tècniques del mòdul Bluetooth JY-MCU HC06 són:
Especificacions
- Alimentació 3.6 – 6 V
- Nivell de senyal 3.3 volts
- Versió de Bluetooth V2.0+EDR
- Taxa de transmissió (per defecte) 9600 baud
- Cobertura de senyal 10 m
- Tipus de comunicació Serial RS232 nivell TTL
- Contrasenya (per defecte) 1234
- Allargada 44 mm
- Amplada 16 mm
- Gruix 7 mm
- Pes 7 g Taula 3.4 Característiques tècniques JY-MCU HC-06 Bluetooth
Figura 3.7 Cara de davant de JY-MCU
Extret de:
http://img.dxcdn.com/productimages/s
ku_104299_1.jpg
Figura 3.8 Cara de darrera de JY-
MCU
Extret de:
http://img.dxcdn.com/productimages/s
ku_104299_2.jpg
Treball Final de Grau Xavier Avila Busquets
18
3.5 Matlab
Matlab és un entorn de computació numèrica i un llenguatge de programació. Creat per la
companyia Mathworks, Matlab permet manipular fàcilment matrius, dibuixar funcions i
dades, implementar algorismes, crear interfícies d’ usuari i comunicar-se amb altres
programes en altres llenguatges. Tot i que s’ especialitza en computació numèrica, una caixa
d’ eines opcional permet usar el motor simbòlic de Maple.
3.6 Arduino IDE
Arduino ens proporciona un software per executar i crear els nostres codis, aquest és Arduino
IDE (Integrated Developement Environment). Aquesta IDE s’ instal·larà en el portàtil, és un
entorn molt senzill d´utilitzar i en el que escriurem el programa que voguem que el Arduino
executi. Una vegada escrit, el carreguerem a través del USB i Arduino començarà a treballar
de forma automàtica.
Figura 3.9 Logotip de Matlab
Extret de: http://www.nersc.gov/assets/Vis/matlablogo.jpg
Figura 3.10 Captura de pantalla d’ Arduino IDE
Treball Final de Grau Xavier Avila Busquets
19
3.6.1 Connexions entre Matlab i Arduino
Per a poder controlar el microcontrolador Arduino des de Matlab és necessari la instal·lació
de unes llibreries. Procedirem a descriure els passos necessaris per a aquesta tasca.
Primer de tot és necessari descarregar els packs de suport d
‘Arduino per a Matlab, http://es.mathworks
.com/matlabcentral/fileexchange/32374-legacy-matlab-and-
simulink-support-for-arduino, descomprimir-los i
emmagatzema’ls a la carpeta ArduinoIO, amés a més del
Software d ‘Arduino de la pàgina oficial:
https://www.arduino.cc/en/Main/Software.
Executem el instal·lador d ‘Arduino 1.6.6 i obrim el programa.
Ens ubiquem en la carpeta “pde” on trobarem els diferents arxius
per instal·lar en la placa Arduino, el que nosaltres necessitarem
serà el arxius “adios.pde” el qual carregarem a la placa
Arduino UNO.
A continuació executarem el instal·lador “install_arduino.m” amb el qual obtindrem totes
les llibreries que ens fan falta per a Matlab.
Figura 3.11 Captura de pantalla
dels arxius descarregats.
Treball Final de Grau Xavier Avila Busquets
20
3.7 FreeCAD
FreeCAD és una modelador de CAD3D paramètric. És programari lliure i orientat a
l'enginyeria mecànica, el disseny industrial i l'arquitectura tot i que té aplicacions en altres
disciplines relacionades.
3.8 GitHub
GitHub és un servei de hosting de repositoris Git, el qual ofereix tota la funcionalitat de Git
de control de revisió distribuït i administració de codi de la font(SCM) així com afegint les
seves característiques pròpies. A diferència de Git, el qual és estrictament una eina de línia
d'ordres, GitHub proporciona una interfície gràfica basada en web i escriptori així com
integració del mòbil. També proporciona control d'accés i diverses característiques de
col·laboració com bug tracking, administració de tasques, i wikis per cada projecte.
GitHub ofereix ambdós plans; per a repositoris privats i comptes gratuïts, els quals són
normalment utilitzats de host en projectes de programari lliure. Del 2015, GitHub informa
que hi ha més de 9 milions d'usuaris i més de 21,1 milions de repositoris, fent-lo el host de
codi de font més gran del món.
Figura 3.12 Logotip de FreeCAD
Extret de: http://forum.freecadweb.org/viewtopic.php?t=9468&start=50
Figura 3.13 Logotip de GitHub
Extret de: http://opendigitalscience.eu/wp-content/uploads/2015/09/github-logo.png
Treball Final de Grau Xavier Avila Busquets
21
4 Disseny del robot
4.1 Consideracions prèvies
Donat que en el món OpenSource existeix una gran varietat de dispositius que realitzen tot
tipus de tasques, el treball s´ha basat en la construcció de un robot controlat de forma
inalàmbrica i amb control de llaç tancat de la velocitat.
El caràcter multidisciplinari per el desenvolupament complet del robot que s´ha dissenyat
ha fet que es tinguin en compte els següents criteris:
Es pot observar com el fluxgrama condueix al preu del producte final. La referencia al preu
no és nomes el preu dels diferents materials i ma d´obra que conformen el producte, sinó
els costos indirectes que comporta la I+D del mateix.
Figura 4.1 Fluxgrama dels criteris per el desenvolupament del robot.
Treball Final de Grau Xavier Avila Busquets
22
Un dels dissenys ja creats amb el que es podia començar a treballar ha sigut el robot
“Renacuajo” de “Thingiverse”. Aquest robot estava dissenyat per a seguir una línia, i esta
equipat amb una placa Arduino UNO, 2 sensors IR, 2 rotors servo continus, 8 piles AAA i
un mòdul Bluetooth opcional. Ha fet falta una completa desconstrucció d´aquest donat que
molts dels components del que esta compost s´han tingut que millorar o obviar.
Figura 4.3 Robot “Renacuajo” de Thingiverse
Extret de:
http://www.thingiverse.com/thing:210098
Figura 4.2 Disseny final del robot
Treball Final de Grau Xavier Avila Busquets
23
4.2 Mecanisme de propulsió
4.2.1 Diferents tipus de motors
4.2.1.1 Servomotors
Un servomotor és un tipus de motor CC petit que te un eix de rendiment controlat. Aquest
pot ser portat a posicions angulars especifiques al enviar una senyal d´entrada. A la pràctica
s’utilitzen per a posicionar palanques, petits ascensors o timons. Els mes coneguts son els
utilitzats en radio-control.
El seu funcionament és relativament senzill, te un circuit de control i un potenciòmetre
connectat al eix. El sistema de control es limita a indicar en quina posició s´ha de situar
mitjançant una senyal analògica. Aquests normalment esta limitats a un moviment de 180º,
limitat per un enclavament, el qual es pot eliminar amb els coneixements necessaris de forma
que es converteixi amb un servo de rotació continua.
La seva velocitat de gir de un servomotor convencional de modelisme és de 60º en 0,12
segons. Si fem el càlcul de les revolucions obtenim:
𝑥 =60 𝑠
1 𝑚𝑖𝑛·
60𝑜
0,12 𝑠= 30000𝑜
𝑚𝑖𝑛⁄ 4.1
𝑟𝑝𝑚 = 30000𝑜
𝑚𝑖𝑛⁄ ·1 𝑟𝑒𝑣𝑜𝑙𝑢𝑐𝑖ó
360𝑜= 83, 3 𝑟𝑝𝑚
4.2
Si fem el càlcul amb una roda de uns 28 mil·límetres de radi obtenim una velocitat màxima:
𝐿 = 2𝜋𝑟 = 2𝜋 · 2,8 𝑐𝑚 = 17,59 𝑐𝑚
4.3
𝑉 = 83, 3 𝑟𝑝𝑚 · 17,59 𝑐𝑚 = 1460 𝑐𝑚 𝑚𝑖𝑛⁄ 4.4
𝑉 = 1460 𝑐𝑚 𝑚𝑖𝑛⁄ ·1 𝑚𝑖𝑛
60 𝑠= 24 𝑐𝑚 𝑠⁄
4.5
Figura 4.4 Servomotor
Extret de: https://at89c52proyect.wordpress.com/2010/09/07/servomotores/
Treball Final de Grau Xavier Avila Busquets
24
4.2.1.2 Motors de continua
El motor de corrent continua, o també anomenat de corrent directe, motor DC o CC, és una
maquina que converteix l´energia elèctrica en mecànica provocant un moviment rotatori
gracies a l’acció que genera un camp magnètic.
Els avantatges d´aquest tipus de motors son: Amplia gama de velocitats, fàcil control, gran
flexibilitat y ampli rendiment. És possible controlar la velocitat de gir d´aquest motors
utilitzant tècniques de control de motors DC.
Les característiques tècniques de un motor DC encarat a se utilitzat en petits robots és de
unes 180 rpm. A primera vista es pot observa la diferencia de velocitat amb un servomotor
el qual arribava a les 83 rpm. Si fem el càlcul de la velocitat obtenim:
𝐿 = 2𝜋𝑟 = 2𝜋 · 2,8 𝑐𝑚 = 17,59 𝑐𝑚
4.6
𝑉 = 180 𝑟𝑝𝑚 · 17,59 𝑐𝑚 = 3166,73 𝑐𝑚 𝑚𝑖𝑛⁄ 4.7
𝑉 = 3166,73 𝑐𝑚 𝑚𝑖𝑛⁄ ·1 𝑚𝑖𝑛
60 𝑠= 52,78 𝑐𝑚 𝑠⁄
4.8
Finalment s´ha optat per la utilització de un motor de continua ja que ofereix unes millors
prestacions i preu.
Figura 4.5 Motor CC
Extret de: http://www.comohacerturobot.com/Taller/taller-
MotoresCC.htm
Treball Final de Grau Xavier Avila Busquets
25
4.2.2 Control dels motors
4.2.2.1 Driver L298N
Donat que és necessària subministrar una potencia major a la que pot oferir la placa Arduino
UNO, s´ha necessita la implementació de un xip L298. Aquest xip consta dos ponts H
complets per a poder controlar tant la direcció com la velocitat de dos motors de continua, a
més de poder controlar motors pas a pas. Aquest xip està integrat amb un driver per a que la
seva utilització i configuració sigui més senzilla.
El seu funcionament es basa en un pont H el qual mitjançant la introducció de un 1 lògic per
“IN1” i un 0 lògic per “IN2” o viceversa aconseguirem que el motor estigui configurat per a
girar en un sentit o el invers. Posteriorment s´habilita el funcionament del motor aplicant una
1 lògic en “ENA” i aconseguirem el gir a tota velocitat del motor.
Figura 4.6 Detall Driver L298N
Extret de: https://www.fabtolab.com/L298N-motor-driver
Figura 4.7 Configuració de un pont complet
Extret de: http://fribotte.free.fr/bdtech/Drivers/PontH.gif
Treball Final de Grau Xavier Avila Busquets
26
La forma de controlar la velocitat és fa mitjançant la introducció de una senyal PWM de
cicle variable. Gràcies a aquest configuració no hi ha cap demanda de potencia del motor al
microcontrolador, fet que podria provocar el sobreescalfament i dany d’aquesta.
Treball Final de Grau Xavier Avila Busquets
27
4.2.2.2 Codificador
Per al control en llaç tancat del motor és necessari la implementació de codificador per a la
lectura de velocitat d´aquests. Existeixen dos tipus de codificador, el absolut i el incremental.
En aquest projecte només s´ha treballat amb codificadors incrementals.
Codificador Absolut
Anomenat així degut a que les posicions del eix estan especificades per el usuari o fabricant.
Això fa que les posicions sempre es mantinguin igual i permet un control molt precís i de
més qualitat de les posicions del eix en tot moment.
Com es pot observar en la figura, aquest codificador consta de 8 posicions diferents les quals
es poden identificar mitjançant la lectura de tres senyals, “A”, “B” i “C”, això ens permet
conèixer en tot moment la posició del eix. La resolució d´aquest dependrà de numero de
senyals que es vulguin llegir.
Codificador incremental
Els codificadors incrementals son més senzills de construir i en conseqüència més
econòmics.
Per al disseny del robot s´ha utilitzat un codificador incremental, donat que no era necessari
tenir una gran precisió en la posició del eix, només necessitem poder realitzar el càlcul de
velocitat. Amb un codificador absolut els mètodes de codificació son bastant incomodes de
treballar.
El codificador incremental proporciona dos formes de ona quadràtiques desfasades 90º, el
qual de forma general son “A” i “B”. Amb la lectura de un sol d´aquests dos canals obtindrem
la informació respecte la velocitat de rotació, i si afegim la segona senyal obtindrem el sentit
de gir.
Figura 4.8 Funcionament de encoder absolut
Extret de: http://profeitm.blogspot.com.es/2016/05/encoders-
incrementales-y-absolutos.html
Treball Final de Grau Xavier Avila Busquets
28
Com es pot observar en la figura, obtindrem una seqüencia diferent en cas de que gires el
rotor a esquerra o a la dreta. Com per exemple tindríem la seqüencia del valor “AB” “10-
11-01-00-10...” en girar en un sentit o la “00-01-11-10-00” a l´inrevés. Això fa que
puguem treballar amb un maquina d´estats.
Figura 4.9 Funcionament de encoder incremental
Extret de: https://www.pmdcorp.com/resources/articles/8
Treball Final de Grau Xavier Avila Busquets
29
4.3 Estudis del motor
Un cop s´ha tingut clar l´objectiu del treball i plantejats els fonaments teòrics, s´ha procedit
a realitzar proves amb l ´Arduino i Matlab, per poder comprendre de forma més eficient el
funcionament entre les dos plataformes. De forma que s´ha pogut entendre millor la forma
d’interactuar cadascuna i facilitar feina per a tasques posteriors.
Això s´ha realitzant mitjançant el estudi del comportament dels motors, fent diverses proves
per a poder conèixer diferents paràmetres d´aquests i optimitzar l’ eficiència del codi i els
elements físics.
Treball Final de Grau Xavier Avila Busquets
30
4.3.1 Programa d ´Arduino
Ha sigut necessari crear un programa per implementar en Arduino, el qual ens permetés la
comunicació entre Arduino-Matlab. Aquest també ha estat crear de forma que es pugui
agafar parts del programa sense tindre que modificar-les.
Per a realitzar el càlcul de velocitat s´utilitzen interrupcions, donat que realitzar un consulta
constant de les variables del codificador resultaria una pèrdua de temps de computació
absurda. Les interrupcions que s´han utilitzat son per temps, per pujada de flanc de una
entrada i per event de comunicació, configurades de la següent manera:
Timer1.initialize(ISR_Time); // Time of interruption
Timer1.attachInterrupt( ISR_Callback) ; // Callback function
attachInterrupt(digitalPinToInterrupt(encA1), encoderCountA1, RISING);
//Interrupt pin, ISR function, Mode
Serial.begin(9600); // Serial Port Communications at 9600 Baud rate
strPwm.reserve(10); // Reserve 10 bytes
Per consultar la resta del codi, vegeu a l´apartat annexes: “Inicialitzacions”
En elles es feia la suma de un incrementador per a poder contar les interrupcions del
codificador per flanc de pujada, i cada 100 ms per a fer el càlcul de velocitat d´aquest, com
es mostra en la següent funció:
int GetVelocity()
int rpm;
int count= prev_countA-countA;
rpm=(60/((float)ISR_Time/1000000))*count/(ticks_per_rev*reduction_gear);
prev_countA=countA;
return rpm;
Per consultar la resta del codi, vegeu a l´apartat annexes: “Funcions control motor”
Es pot observar el càlcul utilitzat per a calcular les revolucions per minut del motor. En el
que passem el valor de temps de la interrupció que esta en microsegons a minuts i
posteriorment, mitjançant una regla de tres, obtenir els valors de la velocitat en rpm.
La interrupció per event de comunicació es realitza guardant tots els valors en un vector, de
la següent manera:
void serialEvent()
Figura 4.10 Captura de pantalla de Arduino IDE per a comunicació amb Matlab
Treball Final de Grau Xavier Avila Busquets
31
while(Serial.available())
data=(char)Serial.read();
if (data != 'z')
Trama[i++]=data;
else
comFinished = true;
x=i;
i=0;
Per consultar la resta del codi, vegeu a l´apartat annexes: “Funcions control motor”.
Tot es mostra per una pantalla LCD implementada per a una millor visualització de les dades
obtingudes a més de ser enviades per el port USB mitjançant:
Serial.println(vel_sens);
Per consultar la resta del codi, vegeu a l´apartat annexes: “Programa principal Arduino”.
Treball Final de Grau Xavier Avila Busquets
32
4.3.2 Anàlisis de les No linealitats del motor
Els motors de corrent continua son els més comuns y econòmics, i es poden trobar en la
majoria de joguines a piles. Per poder analitzar aquests motors es necessita coneixement físic
del sistema, unitats de les constants que apareixen en el model, selecció adequada de les
variables d´estat i a la seva vegada poder observar el comportament, un simulador per a
objecte d´estudi s´ha utilitzat el programa MATLAB.
Realitzarem el estudi experimental per saber si el motor es comporta de forma lineal en tot
el rang d´operació. Generarem una senyal d´entrada com la que veiem a continuació.
Posteriorment s´introdueix en el motor i es recull la informació de velocitat. Per a la
realització d´aquestes tasques s´ha tingut que fer una sèrie d’Scripts en Matlab, al igual que
un programa per al microcontrolador que a continuació exposarem.
La forma en que s´ha escollit la realització del programa ha sigut per funcions, de forma que
es puguin utilitzar aquestes funcions per a diferents programes.
Començarem per la inicialització del port de comunicació, en aquest cas esta connectat
mitjançant el port USB del ordinador la qual cosa fa que sigui molt fàcil la seva configuració.
s = serial('COM4');
Per consultar la resta del codi, vegeu a l´apartat annexes: “Script per a configuració de
comunicació mitjançant Port USB”.
0 5 10 15 20 25 30 35 40 45 50-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
Temps (Segons)
PW
M
Gràfica de entrada per estudi de no-linealitats
PWM (%)
Figura 4.11 Gràfica d´entrada de senyal per a estudi de No-Linealitats
Figura 4.12 Captura de pantalla de les
funcions creades en Matlab
Treball Final de Grau Xavier Avila Busquets
33
Per a tindre una bona comunicació amb el microcontrolador s´ha decidit implementar la
comunicació per trames. De forma que en la trama s´envia en la primera part de la trama el
motor al qual s´envia la informació, posteriorment el PWM i finalment un “EOF” (End Of
Frame o Final de trama).
str = int2str(number); % convert number to string to send send = ['a' str 'z']; % save string to send with SOF and EOF fwrite(s,send,'char','sync'); % Send Frame
Per consultar la resta del codi, vegeu a l´apartat annexes: “Script per a la recollida de dades
de No-Linealitats”.
Per a la correcta recepció de la trama, és necessari enviar tots els valors com un “String”.
Posteriorment s´emmagatzemen les dades en dos vectors, les quals estan normalitzades per
a poder representar una gràfica amb les mateixes escales relatives.
rpm(1:end-1)=rpm(2:end); %Move all string one position back rpm(end)=(fscanf(s,'%d'))/Max_Velocity; % Read velocity and save on the
last position of array
pwm(1:end-1)=pwm(2:end); %Move all string one back position pwm(end)=(number/255); %Save number in the last position of string
(…)
data(:,1)=rpm(:,1); data(:,2)=pwm(:,1); array=timeseries(data); % Create a timeseries of a vector array.TimeInfo.Increment=Ts; %Define interval of timseries Vector
Per consultar la resta del codi, vegeu a l´apartat annexes: “Script per a la recollida de dades
de No-Linealitats
Finalment es bolquen totes les dades en un vector el qual podrem utilitzar en la següent
funció de la creació de la gràfica. I es visualitzen els resultats.
Treball Final de Grau Xavier Avila Busquets
34
Per consultar el codi, vegeu a l´apartat annexes: “Script per a la realització del gràfic del
estudi de No-Linealitats”.
Per consultar el codi d´execució de totes les funcions, vegeu a l´apartat annexes: “Script per
al estudi de No-Linealitats”.
Un cop realitzada la recollida de dades procedim a analitzar-les per tal de conèixer les zones
mortes i de saturació del nostres sistema, les quals mostrem en la següent taula.
Gir horari Gir antihorari
Zona Morta Saturació Zona Morta Saturació
Acceleració 0.364 - 0.411 -
Desacceleració 0.179 - 0.168 - Taula 4.1 Resum de les No-Linealitats
Amb la visualització de les dades es pot observar que la zona se saturació és inexistent, ja
que com s´observa en la gràfica la corba no s’estabilitza en cap moment, i a més, hi ha una
punta de velocitat amb un PWM del 100%, s´han realitzat diferents proves en les quals tots
els resultats han estat similars. Per a la zona de saturació tenim uns valors més aviat alts,
això pot ser degut a que la qualitat del motor és menor degut a que s´ha volgut minimitzar el
cost econòmic del robot. Donat el objectiu del projecte, això no representa un problema.
0 5 10 15 20 25 30 35 40 45 50-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
Temps
%V
elo
citat
i P
WM
Gràfica de no-linealitats
Velocitat (%)
PWM (%)
Figura 4.13 Gràfica d´estudi de No-Linealitats
Treball Final de Grau Xavier Avila Busquets
35
4.3.3 Estudi de l´ample de banda
L´ample de banda d´un sistema de control dona indicacions de les propietats de resposta
transitòries en el domini del temps. També indica les característiques del filtrat de soroll lo
robust que és el sistema, amb el terme de robustesa es vol indicar una mesura de la
sensibilitat del sistema a les variacions de paràmetres.
Per a la creació s´ha utilitzat la funció “createFixedTsSinestream”, la qual crea una senyal
d´entrada per al sistema sinusoïdal en el qual cada freqüència te el mateix temps de mostreig.
On “Ts” és la freqüència de mostreig en radiants per segon i el senyal te 30 freqüències entre
1 i w.
input_arr= frest.createFixedTsSinestream(Ts,0.6, 4*2*pi); % (Ts,sys, w)
input_arr.Amplitude = 0.2; input_arr.NumPeriods = 4;
input= generateTimeseries(input_arr); Data_sinestream=input.data(:,1)+0.5; %Obtenim els punts de la señal i
afegim un offset per treballar a la zona de no linealitats
Per consultar el codi, vegeu a l´apartat annexes: “Script de creació de senyal sinusoïdal de
freqüència variable”.
Finalment es modula l´amplitud per treballar amb una variació de pic a pic de 0.4 i 4
períodes per cada freqüència, s´afegeix un offset per a treballar fora de la zona morta i per
sota del màxim i s´introdueix en el sistema.
S´introdueix la senyal de forma similar que en el apartat de No-Linealitats i es recullen les
dades per al seu tractament.
for i=0:1:10; rpm_no_offset(1,:)=[]; %Esborrar els primers 10 valors Data_no_offset(1,:)=[]; end
rpm_no_offset=detrend(rpm_no_offset,'constant'); %Treure offset de la
senyal de rpm Data_no_offset=detrend(Data_no_offset,'constant'); %Treure offset de la
senyal de rpm
0 0.5 1 1.5 2 2.5 3 3.5
x 104
0.3
0.35
0.4
0.45
0.5
0.55
0.6
0.65
Senyal d´exitació amb Offset
t (seg)
DC
Figura 4.14 Senyla d´exitació sinusoïdal de freqüència variable
Treball Final de Grau Xavier Avila Busquets
36
Per consultar el codi, vegeu a l´apartat annexes: “Tractament de senyal de sortida del ample
de banda”.
A les quals es retiren les primeres mostres, donat que en l´arrancada de qualsevol sistema
pot induir a error el càlcul dels paràmetres en regim estacionari d´aquest.
Com podem observar tenim una disminució de l´amplada de la senyal al voltant dels 4 Hz.
Procedim a la realització de transformada de Fourier discreta del sistema amb les funcions
de Matlab:
NFFT=2^nextpow2(L); Y=fft(rpm_no_offset,NFFT)/L; f=Fs/2*linspace(0,1,NFFT/2+1);
Per consultar el codi, vegeu a l´apartat annexes: “Script de la creació de Fast Function
Transfer and plot”.
0 50 100 150 200 250 300 3500
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9RPM
Temps (s)
Velo
citat
(rpm
)
0 50 100 150 200 250 300 3500.3
0.35
0.4
0.45
0.5
0.55
0.6
0.65
PWM
Temps (s)
PW
M (
%)
Figura 4.15 Entrada i sortida del sistema per a ample de banda
Treball Final de Grau Xavier Avila Busquets
37
Com podem observar el resultat és correcte ja que s´estabilitza al voltant del 4Hz.
Llavors sabem que el motor no és capaç de treballar amb una freqüència de entrada de la
senyal de fins a 4 Hz sense que aquesta quedi disminuïda.
Per a l´obtenció del bode només és necessari la execució de la funció per a crear una funció
de transferència estimada.
Ft = tfest(signals,1);
Per consultar el codi, vegeu a l´apartat annexes: “Script de creació de funció de transferència
del sistema”.
Ft =
From input "u1" to output "y1":
28.56
---------
s + 30.71
0 2 4 6 8 10 12
0
0.005
0.01
0.015
0.02
0.025
0.03
0.035
Single-Sided amplitude Spectrum of y(t)
Frequency (Hz)
|Y(f
)|
Figura 4.16 Gràfica de la Funcio de Fourier Discreta
-40
-30
-20
-10
0From: u1 To: y1
Magnitude (
dB
)
100
101
102
103
-90
-45
0
Phase (
deg)
Bode Diagram
Frequency (rad/s)
Figura 4.17 Gràfica de Bode
Treball Final de Grau Xavier Avila Busquets
38
Continuous-time identified transfer function.
Parameterization:
Number of poles: 1 Number of zeros: 0
Number of free coefficients: 2
Use "tfdata", "getpvec", "getcov" for parameters and their
uncertainties.
Status:
Estimated using TFEST on time domain data.
Fit to estimation data: 67.27% (simulation focus)
FPE: 0.002144, MSE: 0.002143
Resultat visualitzat en Matlab de la realització de la funció de transferència.
Treball Final de Grau Xavier Avila Busquets
39
4.3.1 Polinomi de la velocitat
Per a l´estimació del polinomi de la velocitat es crea un programa en Arduino el qual va
augmentant el PWM i enviant el valor de velocitat per el port Serial. Per a visualitzar el
programa d ‘Arduino anar al apartat de Annexes
p = polyfit(pwm,y_rpm,2);
Per consultar la resta del codi, vegeu a l´apartat annexes: “Script per la linealització de la
velocitat”.
Amb les dades reals s´ha intentat calcular el polinomi de velocitat de sogon ordre. Polyfit
busca el coeficient del polinomi p(x) del grau n per aproximar les dades en forma quadràtica.
Amb el qual hem obtingut els valors:
𝑝(𝑥) = −0.0029𝑥2 + 1,6217𝑥 − 28,4256 4.9
I a continuació mostrarem la gràfica d´aproximació del model de velocitat.
100 120 140 160 180 200 220 240 26080
100
120
140
160
180
200Gràfica de linealització de la velocitat
Temps (Segons)
Velo
citat
Motor
Linealitzat
Figura 4.18 Gràfica de linealització de la velocitat
Treball Final de Grau Xavier Avila Busquets
40
4.3.2 Estimació de paràmetres del motor
La modelització del motor es refereix al procés de trobar l´aproximació lineal a una funció
en un punt donat. En estudis dels sistemes dinàmics, és un mètode per estudiar l´estabilitat
local en un punt d´equilibri de un sistema d’equacions diferencials no lineals.
El circuit elèctric equivalent de l´armadura i diagrama en buit del rotor es pot observar en la
següent figura.
Per a aquest sistema, haurem d´assumir que la entrada del sistema és la font d´entrada (V)
aplicada a l´armadura del motor, mentre que la sortida és la velocitat de rotació del eix 𝑑𝜃
𝑑𝑡.
Assumim que el eix i el rotor son rígids. A més a més, assumirem una fricció viscosa del
model, la fricció de par és proporcional a la velocitat angular del eix.
En general, el par generat per el motor és proporcional al corrent d´armadura i la força del
camp magnètic. En aquest estudi assumirem que el camp magnètic és constant i, per tant,
que el par del motor Ƭ és proporcional només al corrent d´armadura i per el factor KƬ com
es mostra en l´equació següent:
𝜏 = 𝐾𝜏 · 𝑖 4.10
La força contraelectromotriu e és proporcional a la velocitat angular del eix per el factor Ke.
𝑒 = 𝐾𝑒 · 𝜃 4.11
En unitats del sistema internacional, el par motor i la força contraelectromotriu son iguals,
llavors 𝐾𝜏 = 𝐾𝑒, doncs farem servir K per representar els dos.
Mitjançant les equacions de par i la malla de Kirchoff trobem:
𝑉 = 𝑅 · 𝑖 + 𝐿𝑑𝑖
𝑑𝑡+ 𝑒
4.12
Figura 4.19 Circuit elèctric equivalent de motor de continua
Extret de: http://physics.stackexchange.com/questions/107568/quantifying-
the-current-drawn-by-a-motor-at-half-speed
Treball Final de Grau Xavier Avila Busquets
41
Si fem el sistema d´equacions:
𝐽𝑑𝜔
𝑑𝑡+ 𝑏 · 𝜔 = 𝐾𝑖 · 𝑖
𝑉 = 𝑅 · 𝑖 + 𝐿𝑑𝑖
𝑑𝑡+ 𝐾𝑒 · 𝜔
4.13
Expressem en espai d´estats, on els estats seran 𝜔 i i.. Recordant que un espai d´estats
s´expressa de la següent manera.
= 𝐴𝑥 + 𝐵𝑢𝑦 = 𝐶𝑥 + 𝐷𝑢
𝑝𝑒𝑟 𝑎 𝑑𝜔
𝑑𝑡= ,
𝑑𝑖
𝑑𝑡= 𝑖
4.14
Amb l’equació inferior, podrem derivar la següent equació basant-nos en la segona llei de
Newton i la llei de voltatge de Kirchoff.
𝐽𝑑𝜔
𝑑𝑡= −𝑏 · 𝜔 + 𝐾𝑖 · 𝑖
𝐿𝑑𝑖
𝑑𝑡= 𝑉 − 𝑅 · 𝑖 − 𝐾𝑒 · 𝜔 − 𝑉
4.15
L’equació inferior pot ser expressada triant la velocitat de rotació i corrent elèctric com a
variables d´estat. Una vegada més el voltatge d´armadura és l´entrada i la velocitat de rotació
com la sortida.
𝑑
𝑑𝑡[𝜔𝑖] =
[ −
𝑏
𝐽
𝐾
𝐽
−𝐾
𝐿−
𝑅
𝐿] · [
𝜔𝑖] + [
01
𝐿
]𝑉 4.16
𝑦 = [1 0] [𝜔𝑖]
4.17
Per a l´estimació de paràmetres és aportar una senyal d´entrada quadràtica de freqüència 1Hz
i guardar la velocitat del motor. Una vegada aconseguides les dades ces crea una funció de
Matlab per calcular a quan estem del model real, aquesta funció agafa l´entrada, els
paràmetres i calcula la sortida, llavors compara amb la sortida real amb l´estimada per a
aconseguir números positius els quals és la distancia amb la sortida real.
newParams = fminsearch(@distance,params)
Per consultar la resta del codi, vegeu a l´apartat annexes: “Script per a l´obtenció dels
paràmetres del motor”.
Fminsearch busca el mínim de una funció escalar de moltes variables, començant per un
valor estimat. Aquesta és generalment referida com una Optimització no-lineal no constant.
Treball Final de Grau Xavier Avila Busquets
42
Finalment hem aconseguit els valors del motor, els quals son:
𝐽 = 6,0442 · 10−8 𝐾𝑔 · 𝑚2 𝐾 = 1,0162 𝑉 · 𝑠𝑟𝑎𝑑⁄ 𝐿 = 2,4849 𝑚𝐻
𝑏 = 0,0212 𝑁 · 𝑚(𝑟𝑎𝑑
𝑠⁄ )⁄ 𝑅 = 0,911 Ω
I la següent gràfica de l´aproximació al model:
En la gràfica s´observa la resposta temporal a un esglaó real, la senyal del motor real en blau
i la senyal del nostre model en vermell. Com observem l´aproximació és prou bona com per
donar-la per correcte. El rissat és un component de soroll no modelada.
Per a visualitzar el programa d ´Arduino anar al apartat de Annexes “Codi per a la
linealització de la velocitat”.
0 5 10 15 20 25 30 35 400
20
40
60
80
100
120
140
160
180
200Gràfica de parametres estimats
Temps (Segons)
Velo
citat
ymodel
y
Figura 4.20 Gràfica d´estimació de parametres
Treball Final de Grau Xavier Avila Busquets
43
4.4 Sistema de control PID
Un controlador PID és un mecanisme de control per realimentació usat en sistemes de
control industrial. Aquest calcula la desviació o error entre un valor mig i el desitjat.
El algoritme de control consisteix en tres paràmetres diferents, el proporcional, el integral i
el derivatiu. El valor proporcional depèn del error actual. L´integral depèn dels errors de
passat i el derivatiu és una predicció dels errors futurs. La suma dels tres es usada per ajustar
el procés per mitja d´un element de control, en el nostre cas la potencia subministrada.
L´acció proporcional de Kp fa que l´error en estat estacionari tendeixi a 0, l´integral Ki al
anar sumant el error en el temps, tendeix a eliminar el error estacionari generat per l’acció
proporcional, i finalment Kd tendeix a suavitzar les oscil·lacions en el error.
Figura 4.21 Diagrama de blocs d´un control PID
Extret de: https://ca.wikipedia.org/wiki/Proporcional_integral_derivatiu
Treball Final de Grau Xavier Avila Busquets
44
4.4.1 PID en el Arduino
Actualment existeixen llibreries de control PID per a Arduino completament explicades les
quals pots accedir i modificar al teu gust. En el present projecte s´ha optat per a la creació
de la nostre pròpia estructura basada en altres programes proporcionats per la comunitat
OpenSource.
Seguint amb la manera de treballar anterior s´han utilitzat parts de codi que poden se
utilitzades per varies funcions sense que aquest afecti els uns als altres. D´aquesta manera
s´ha creat la estructura de dades següent:
struct pid_data
float kp, ki, kd;
float delta;
float sum;
float prevError;
;
(...)
struct pid_data pid_data_R, pid_data_L;
(...)
control_pid_ini(pid_data_R, 0.01, 4, 0.1, 0); //Valors de PID aproximats
control_pid_ini(pid_data_L, 0.01, 4, 0.1, 0);
Per consultar el codi, vegeu a l´apartat annexes: “Programa principal de PID”.
En el “Struct” es pot accedir als diferents valors de K, el proporcional, integral i derivatiu,
“delta” que és el temps de mostreig del sistema, “sum” que serà la suma de tots els errors
amb les respectives K i finalment el “prevError” que és el error que s´ha tingut en el mostreig
anterior, necessari per al càlcul integral del PID. A continuació es declara les estructures per
a cada motor i es procedeix a la inicialització de les variables com podem observar en el codi
següent:
void control_pid_ini(struct pid_data &pid, float delta, float kp, float
ki, float kd)
pid.delta = delta;
pid.kp = kp;
pid.ki = ki;
pid.kd = kd;
pid.sum = 0;
pid.prevError = 0;
Per consultar el codi, vegeu a l´apartat annexes: “Funcions del PID”.
Procedim al càlcul del PID en una funció la qual passem el valor de error i la direcció de
l´estructura PID que utilitzarem:
float Get_pid(struct pid_data &pid, float error)
//Calcul de P
float p = pid.kp * error;
//Calcul de I
pid.sum += error;
float i = pid.ki * pid.delta * pid.sum;
//Calcul de D
float d = pid.kd * (error - pid.prevError) / pid.delta;
Treball Final de Grau Xavier Avila Busquets
45
pid.prevError = error;
float _pid=p+i+d;
//Limitació de valors
if(_pid<0) _pid=0;
if(_pid>255) _pid=255;
return _pid;
Per consultar el codi, vegeu a l´apartat annexes: “Funcions del PID”.
I finalment la execució de la interrupció per Timer, de forma que s´executi a temps regulars
de 100 milisegons:
void ISR_Callback()
detachInterrupt(digitalPinToInterrupt(encAR)); //Disable interrupt
when calculating
detachInterrupt(digitalPinToInterrupt(encAL));
//Right
vel_sensR= GetVelocityR();
float error = (vel_refR - vel_sensR);
PWM_R=(int) Get_pid(pid_data_R, error);
if (gir_R) PWM_R=-PWM_R;
run_motorR (PWM_R);
//Left
vel_sensL= GetVelocityL();
error = vel_refL - vel_sensL;
PWM_L=(int) Get_pid(pid_data_L, error);
if (gir_L) PWM_L=-PWM_L;
run_motorL (PWM_L);
attachInterrupt(digitalPinToInterrupt(encAR), encoderCountAR, RISING);
//Interrupt pin, ISR function, Mode
attachInterrupt(digitalPinToInterrupt(encAL), encoderCountAL, RISING);
Per consultar el codi, vegeu a l´apartat annexes: “Funcions d´interrupció”.
Ha sigut necessari la inhabilitació de les interrupcions dels codificadors per a una millor
computació i fiabilitat del sistema.
Treball Final de Grau Xavier Avila Busquets
46
4.4.2 Obtenció dels valors del PID
En la comunitat OpenSource existeixen programes amb els quals pots ajustar els valors del
PID de forma automàtica, llibreries com “Arduino PID Autotune Library”. Aquestes
treballaven amb llibreries ja creades per a l´execució de PID. En el cas d´aquest projecte al
haver creat un codi personalitzat s´ha optat per altres vies d´obtenció d´aquest.
El ideal per determinar els valors de cada una de les constants es realitzar un estudi
mitjançant les transformades de Laplace y buscar la millor ubicació per als pols i zeros.
En aquest cas s´ha optat per realitzar probes empíriques amb valors baixos i anar provant
diferents combinacions. Aquest mètode d´ajustament consisteix en establir primer el valors
de la integral i la derivada a 0. A continuació, s´incrementa P fins que el motor comenci a
oscil·lar. Llavors s´estableix una P aproximadament a la meitat del valor configurat. Després
incrementem la integral fins que el procés s´ajusti al temps requerit, amb comte de que
augmentar molt Ki pot portar a inestabilitat. Finalment incrementem, si es necessari, fins que
el motor assoleixi una variació el suficient ràpid en una variació brusca de velocitat.
Treball Final de Grau Xavier Avila Busquets
47
4.5 Sistema de comunicació
El sistema de comunicació que s´ha implementat en aquest projecte ha estat la comunicació
Bluetooth, ja que és una forma senzilla i ràpida de comunicació entre tot tipus de dispositius
diferents. En el projecte és necessària la comunicació Bluetooth per a la recepció de dades
de velocitat i diferents sensors i l´enviament de dades al Arduino com a control de velocitat
i direcció.
El terme Bluetooth és una especificació industrial per a Reds Inàlambriques d’Àrea
Personal (WPAN) que possibilita la transmissió de dades entre diferents dispositius
mitjançant un enllaç per radiofreqüència en la banda ISM (Industrial, Scientific and
Medical; bandes utilitzades internacionalment per us no comercial) dels 2.4 GHz. Les
comunicacions Bluetooth defineixen un canal de comunicació a un màxim de 720 kbit/s
amb un rang òptim de 10 m.
Figura 4.22 Logotip i dibuix de comunicació Bluetooth
Extret de: http://www.prometec.net/indice-bluetooth/
Treball Final de Grau Xavier Avila Busquets
48
4.5.1 Configuració del mòdul Bluetooth
Un dispositiu Bluetooth pot treballar com a Master o com Slave. La diferencia és que un
Bluetooth Slave només pot connectar-se a un master, mentre que un master pot estar
connectat a varis Slave. Cada un dels dispositius és identificat via Bluetooth amb una
direcció única de 48 bits, a més de un nom de dispositiu per a poder ´l identificar.
Com que el Bluetooth el va desenvolupar Nokia per connectar telèfons mòbils, entre altres
dispositius, existeix un procediment definit per vincular els diferents dispositius anomenat
“Pairing”. El qual consisteix en demana un numero PIN per autoritzar la connexió. Aquest
procediment només és necessari la primera vegada que es vincula un dispositiu, ja que cada
dispositiu guarda la direcció del altre i connectar-se automàticament.
El model JY-MCU HC-06 disposa de 4 pins(Alimentació (+/-), enviament i recepció).
Aquest model només pot treballar com esclau i disposa de pocs comandes AT per a la seva
configuració.
Serà necessari connectar el mòdul amb els pins de tensió als de 5V i GND de la nostra placa
Arduino i els pins de Tx i Rx en alguns dels pins digitals. És necessari saber que en cas de
que es connectin en els pins Tx i Rx (0 i 1)del microcontrolador aquets hauran de estar girats,
de tal forma que el pin Tx del Bluetooth estarà connectat al Rx del microcontrolador. També
que si s´ha optat per aquesta última opció, en cas de connectar el microcontrolador per USB,
aquests pins hauran d´estar desconnectats, ja que s´utilitzen per a la comunicació entre
Arduino i PC. La configuració més senzilla és com s´indica en la següent figura:
Si la connexió d´alimentació és correcta començarà a parpellejar un LED, quan el mòdul
quedi vinculat a algun dispositiu aquest quedarà encès fixe.
Farà falta la carrega del arxiu que esta en els annexes “Codi configuració Bluetooth”. I amb
l´enviament de comandes AT, les quals es poden visualitzar en el Annex “Comandes AT per
Bluetooth HC-06” mitjançant el monitor sèrie de Arduino IDE.
Figura 4.23 Connexio entre Bluetooth i Arduino UNO
Extret de: http://www.prometec.net/bt-hc06/
Treball Final de Grau Xavier Avila Busquets
49
4.5.2 Comunicació entre Arduino i Matlab
Per a la comunicació entre dos sistemes inalàmbrics és necessari crear un protocol de
comunicació entre ells. El protocol desenvolupat es tracta d´un protocol a mida que
implementa un format de trames similar a les d´un protocol estàndard.
Aquest esta format per un primer paquet en el que s´envia a quin motor esta destinada la
informació, seguit de la comanda (llegir o modificar) i en cas de modificar el valor de
velocitat s´envia el valor desitjat, per acabar un final de trama o EOF, un valor que sigui
impossible que es pugui repetir en alguna posició de la trama, en aquest projecte utilitzem la
lletra “z”.
4.5.2.1 Comunicació Bluetooth desde Matlab
La comunicació Bluetooth per Matlab és molt senzilla, un cop el ordinador ha detectat el
dispositiu USB només és necessari utilitzar dos comandes de Matlab per a establir la
connexió.
%Connect via Bluetooth serial_port = Bluetooth('XURRA_BT',1); fopen (serial_port);
(...)
fclose (handles.serial_port);
Per consultar el codi, vegeu a l´apartat annexes: “Inicialització del port”.
4.5.2.2 Comunicació Bluetooth desde Arduino
La comunicació Bluetooth d ´Arduino es fa com si es rebés les dades per un port sèrie
normal, amb la qual cosa fa que la seva implementació sigui senzilla.
void serialEvent()
while(Serial.available())
data=(char)Serial.read();
if (data != 'z')
Trama[i++]=data;
else
comFinished = true;
x=i;
i=0;
Per consultar la resta del codi, vegeu a l´apartat annexes: “Funcions control motor”.
Motor
SeleccionatComanda Dades EOF
Figura 4.24 Trama d´enviament a Arduino desde Matlab
Treball Final de Grau Xavier Avila Busquets
50
5 Construcció del robot
5.1 Primers dissenys
En aquest punt es descriurà els aspectes constructius que han dirigit el disseny del robot per
buscar les possibles opcions que conclouran en el disseny final.
5.1.1 Disseny del xassís principal
Partint del disseny del Renacuajo vist en el apartat “Consideracions prèvies” s´ha decidit que
l´amplada i allargada del robot no es modificaria.
Aquest primer disseny estava encara al aprofitament del espai al màxim. Les principals coses
a destacar estarien en que el sensor de velocitat està a l´altura del terra, es va tindre que
desestimar aquesta possibilitat, donat que per a futurs projectes, el robot ha de ser capaç
d’espitjar objectes, i en cas d´haver el objecte el sensor de distancia donaria una lectura
errònia.
També es va modifica el suport de darrera, ja que un rodament com una boleta de vidre
provoca menys fricció amb el terra.
I finalment la situació de les rodes es va tindre que avançar, donat que es va tindre que afegir
una reductora al motor, a causa de les altes revolucions que obtenia.
5.1.2 Disseny dels engranatges
Per als primers dissenys del sistema de engranatge entre roda i motor es va optar a un
engranatge cònic, fet causat al poc espai en el xassís per als motors.
Figura 5.1 Primer disseny del xasis del robot
Figura 5.3 Engranatge de roda Figura 5.2 Engranatge del motor
Treball Final de Grau Xavier Avila Busquets
51
Aquests engranatges van tindre que ser modificat diverses vegades, degut a que la precisió
de la impressora utilitzada era baixa i va ocasionar molts problemes.
Treball Final de Grau Xavier Avila Busquets
52
5.2 Mètode d’impressió de les peces
El desenvolupament del disseny del robot s´ha buscat un mitja de producció assequible, tant
a nivell de desenvolupament del producte, com per el possible desenvolupament final, ja que
en funció dels materials utilitzats seria necessari el us de proveïdors externs que fabriquessin
les diferents peces en altres materials.
S´ha plantejat la impressió 3D com la idònia, per un costat per el baix preu d´aquest tipus
d´impressora en els últims anys, i per altre, gracies al seu baix cost, la popularitat que ha
tingut respecte l´obtenció d´una impressora 3D per a ús particular.
Abans d´optar per un material d´impressió u altre, s´ha de tindre una visió clara del resultat
desitjat, ja que les característiques del mateix influiran de manera considerable en el resultat
i en el tractament posterior de la peça impresa, sigui per el seu grau de solidesa, flexibilitat,
resistència, nivell de detall o superfície d´acabat.
Al contrari del que es cregui, la impressió 3D també permet guanyar temps de producció,
encara que el temps d´impressió , com a norma general, és més lent que la resta de mètodes
de fabricació, el temps mig de construcció de tot el robot és menor.
A més a més, el baix preu del material utilitzat, en aquest cas PLA o Àcid Polí làctic, que
permet un tipus de assaig de prova i error, que encara no sigui el tipus d´assaig més òptim,
a nivell temporal i material, és un dels més adequats en aquest tipus de projectes, ja que
depenen de la forma d´impressió o modelat del robot existeixen diferents solucions que no
encareixen el resultat final.
Per a una bona impressió convé escollir be el procés que es vol seguir. Així doncs, utilitzant
el mètode de deposició de material fos s´ha de preveure la disposició de les diferents peces.
La deposició de material fos és el mètode més popular per impressió 3D, on la impressora
crea el model al dipositar un fi traç damunt un altre al que es va adherint el material, per
acció de la temperatura es torna mal·leable, i al funcionar el extrusor a mode d’èmbol fa
que el mateix material vagi espenyent a través de la boquilla fins a depositar-se sobre el
suport.
Figura 5.4 Impressió per deposició de
material fos.
Extret de:
http://tallerdesoluciones.blogs.inti.gob.ar/20
09/09/page/2/
Treball Final de Grau Xavier Avila Busquets
53
El principi d´aquest sistema consisteix en que es treballa per capes de material, una damunt
de l´altre, que es va endurint fins a formar la peça prèviament dissenyada amb CAD. El
problema que produeix aquest mètode és l´aparició de betes a les peces, ja que, encara que
les capes de material s´uneixen entre sí, la unió no és completa.
Per últim esmentar que és necessari retirar els suports creats automàticament per la
impressora, mitjançant un cúter i paciència.
Figura 5.5 Procés d´impressió de les peces
Figura 5.6 Final del procés d´impressió de les peces
Treball Final de Grau Xavier Avila Busquets
54
5.3 Muntatge de les peces
Al ensamblar tot el conjunt de peces per a formar el robot, s´ aprecia que no hi ha tanta
facilitat amb la que es realitza el muntatge d´aquest, donat que estava dissenyat per a que
tota la tornilleria estigués fixada mitjançant femelles, s´ha vist que, a causa del tipus de
impressió, es poden fixar en les mateixes peces, fent que el muntatge d´aquest sigui més
còmode per a futures modificacions. Sempre anant en compte de no apretar de forma
exagerada ja que podríem passar la rosca creada en les parets de cada forat i tindre que buscar
noves formes d’anclatge d´aquell cargol.
El ensamblatge de tot el robot és molt senzill, es pot començar per la inserció del driver
L298N en el xassís, mitjançant 4 cargols de mètrica 3 per 10 mm d´allargada. Com es mostra
en la següent figura.
S´agafen els dos rodament tipus 688Z i introduir els engranatge de la roda. Aquesta operació
s´ha de fer amb precaució, utilitzant una llima de paper s´afinaran les parets del eix per a un
millor desplaçament en la col·locació, i en cas de q fos precís, lubrica’l amb una mica d´oli.
I posteriorment es procedirà a la introducció d´aquests, per la part interior del xassís a les
seves respectives ubicacions. Mitjançant unes Alicates bec de lloro pot ser més fàcil, sempre
amb compte de no doblegar el eix.
Amb un petit martellet s’introdueixen el engranatges en el eix motor i s’ensamblen els
motors amb els seus suports.
Figura 5.7 Ensamblatge del driver L298N
Figura 5.8 Ensamblatge de
engranatge amb rodament
Treball Final de Grau Xavier Avila Busquets
55
Es fan les connexions dels cables del control dels motors al driver L298N.
Es fiquen els neumàtics a les rodes i s’instal·len les dos rodes mitjançant dos cargolets
petits rosca xapa al eix del engranatge i la canica de 8 mil·límetres a la part posterior
.
Ara es pot procedir amb el ensamblatge del xassís superior amb 4 cargols de mètrica 3 i
200 mil·límetres de llargada. Amb el mòdul Bluetooth i el microcontrolador Arduino.
Figura 5.10 Ensamblatge motor i engranatge Figura 5.9 Ensamblatge motors amb suports
Figura 5.11 Ensamblatege de rodes i canica
Figura 5.12 Ensaamblatge del xassís superior
Treball Final de Grau Xavier Avila Busquets
56
Es poden instal·lar els demés elements per al que esta equipat el robot, com mòdul WIFI i
sensors d’ultrasons a la part davantera i posterior. També és necessari soldar les fileres de
pins laterals per així poder puntejar i obtindré varies entrades de GND, 5 Volts i 3,3 Volts
del microcontrolador Arduino.
Finalment es col·loquen les bateries a la part superior del xassís, mitjançant 4 cargols de
mètrica 6 amb 600 mil·límetre de llargada, en els forats dels caps dels cargols que fixen el
xassís superior amb el inferior.
Figura 5.13 Montatge final del robot en Freecad
Treball Final de Grau Xavier Avila Busquets
57
5.4 Pressupost
Una de les premisses que marquen aquest TFG és que els costos de disseny i producció
siguin assequibles a qualsevol usuari i que es pugues reproduir amb facilitat.
En la següent taula es recull un pressupost del prototip bàsic de manera orientativa.
Concepte Model Fabricant Preu/ Unitat
o hora Unitats Preu
Impressió 3D
Xassís superior - 6,00 € 3,61 21,66 €
Xassís - 6,00 € 6 36,00 €
Engranatge de roda - 6,00 € 1 6,00 €
Engranatge de motor - 6,00 € 1 6,00 €
Roda - 6,00 € 1,54 9,24 €
Suports motors - 6,00 € 1,68 10,08 €
Rodament 688z YAZ 1,57 € 2 3,14 €
Motor DC high Torque 25mm
DC 6.0V DX 11,01 € 2 22,02 €
Bluetooth JY-MCU Phantom YoYo 5,77 € 1 5,77 €
Cables DIY F-F DuPont 1,51 € 1 1,51 €
Pins 2.54mm 40-Pin Fem DuPont 2,20 € 1 2,20 €
Sensor ultrasons HC-SR04 OEM 1,79 € 2 3,58 €
Microcontrolador UNO Arduino 24,20 € 1 24,20 €
Driver motor L298N Keyes 4,29 € 1 4,29 €
Tornilleria Variada - 1,89 € 1 1,89 €
Piles recarregables Extreme-Blister Energizer 31,11 € 2 62,22 €
Total 219,80 € Taula 5.1 Taula de Pressupost
Treball Final de Grau Xavier Avila Busquets
58
6 Problemes i solucions Han sigut varis els problemes que s´han anat trobant al llarg del desenvolupament del
projecte, alguns deguts a errors humans i altres per dificultats inherents en la naturalesa dels
elements utilitzats per a la construcció del robot.
En el que fa a errors humans n´hi ha hagut, unes vegades referent al balanç de la càrrega en
el robot com en la creació del software. Els referents als errors en el ensamblatge seran els
que es comentin en més profunditat al llarg d’aquest apartat.
Treball Final de Grau Xavier Avila Busquets
59
6.1 Errors en comunicació
L´objectiu en aquest projecte ha sigut la creació de un robot el qual pogués ser controlat de
forma inalàmbrica, per a posteriorment automatitza el moviment d’aquest.
Les primeres proves de comunicació es realitzaven mitjançant components WiFi, de manera
que ampliaríem la distancia de maniobra del robot. Les proves en comunicació WiFi van ser
correctes i sense problemes. El problema que es va tindre va ser la implementació en Matlab,
ja que la comunicació mitjançant Scripts esta molt poc desenvolupada, en comptes de
mitjançant Simulink, la qual esta més preparada però et limita a un disseny de blocs el qual
pot ser problemàtic per la varietat limitada d´objectes. Fet que fa que per a posteriors
evolucions del robot no es limiti els components que s’hi poden integrar, com per exemple
la visió artificial.
Mitjançant la comunicació WiFi hi va haver problemes d´alimentació, de connexió i de
enviament i recepció d’informació. Començarem parlant de l´alimentació, el mòdul utilitzat
per a dur a terme aquesta va ser el mòdul ESP-01 ESP8266.
L´alimentació del mòdul és de 3,3 Volts, en aquest sentit no hi ha cap problema perquè
Arduino te dissenyades sortides d´alimentació de 3,3 i 5 Volts. El problema esta en el corrent
màxim que pot subministrar, que és al voltant del 50 mA, en el moment de l’arrancada del
mòdul aquesta corrent és bastant superior a aquests 50 mA, de manera que pot portar que no
s’iniciï el mòdul. És recomanable utilitzar una font externa per a la seva alimentació, en el
robot era molt poc pràctic aquesta solució.
Per a la connexió del mòdul un cop arrencat es feia complicat la detecció d´aquest, al igual
que la seva configuració, fet que no va ser molt problemàtic posteriorment però que
s´accentuava amb els problemes de l´alimentació.
Finalment per a l’enviament i recepció d’informació el mòdul enviava les dades de direcció
IP i altres juntament amb la informació que es volia enviar. Això feia que fos molt difícil i
amb un cost molt temps en la neteja per part del microcontrolador de tota la informació
rebuda.
Per tots aquests problemes es va desestimar la connexió mitjançant WiFi i es va optar per
una comunicació més senzilla com la Bluetooth.
Figura 6.1 Módul Wifi ESP-01 ESP8266
Extret de: http://rubensm.com/wp-
content/uploads/2015/01/esp8266.jpg
Treball Final de Grau Xavier Avila Busquets
60
6.2 Impressió de les peces
Un cop fet el primer disseny del robot era necessària la seva impressió mitjançant una
impressora 3D. La universitat em va permetre imprimir el robot en la impressora 3D que
tenien, però degut a retards en la impressió, un error personal en el enviament de la primera
prova i finalment la ruptura de un dels components de la impressora es va tindre que
desestimar la utilització d’aquesta i buscar un proveïdor extern.
Un dels principals problemes van ser els terminis en que vaig tindre el primer disseny del
robot creat i les primeres peces operatives. Degut a la sobrecarrega de treball del meu tutor,
el qual feia d’interlocutor entre el responsable de la impressió de les peces i jo, el termini de
les respostes i dates d’ impressió es va anar allargant, amb la impossibilitat de fer proves de
manejabilitat del robot. També cal esmentar que en la primera impressió hi va haver un error
humà personal, causat per la conversió de els fitxers del format Freecad “.fcstd” a “.std”, ja
que nomes es van passar les parts exteriors del model fent que aquest estigués buit per dins.
Finalment es va optar per a buscar un proveïdor extern per a imprimir les peces, les quals es
van poder imprimir l’endemà de Santo Joan, quan estaven dissenyades des de finals de març.
La impressió de les peces amb el proveïdor extern també va tindre els seus problemes. Degut
a la poca precisió utilitzada per la impressió, es va trobar que diverses mides havien variat,
com per exemple les mides dels forats dels cargols, o l´espai útil de la caixa del xassís. La
solució va ser el canvi de la fixació dels cargols que ensamblaven el xassís inferior i superior,
directament al mateix xassís, aquest fet va ser satisfactori ja que feia un ensamblatge més
senzill del robot, el qual no s´havia contemplat anteriorment. Per al espai útil en la caixa va
ser molt més complicat, degut a aquesta disminució, la connexió entre els dos engranatge,
de la roda i el motor, es va tindre que redissenyar diverses vegades. En cada disseny es tenia
vigilar molt be l’ impressió de les peces, degut a que la calor ambiental feia que els eixos no
quedessin rectes i tindre problemes de tracció, es va optar per varies solucions bastant
rudimentàries. Una d’elles va ser ficar un ventilador en la base d’impressió per a un refrigerat
més òptim de la peça i evitar deformacions en la impressió. un altre de les solucions va ser
que els suports dels motors no estiguessin apretats completament, això feia que el motor
tingués una mica de moviment dintre el suport i permetia una millor connexió i treball entre
els dos engranatges. També es va deixar els rodaments del eix de la roda sense fixar, ja que
en un primer disseny aquests estaven recolzats en una pestanya en el xassís, per evitar el
moviment d´aquests, amb l’eliminació d’aquestes pestanyes va fer que els rodaments
tinguessin una mica més de marge per a treballar. Totes aquestes modificacions van fer que
l’ alineació de les rodes quedes mermada i una d´elles no quedés totalment recte, provocant
que el robot no seguís una línia recta perfecte.
Treball Final de Grau Xavier Avila Busquets
61
6.3 Balanç de càrregues
En els primers dissenys del robot es va optar per a la utilització dels motors sense reductora,
això feia que quedessin les rodes més centrades en el model, això juntament que les bateries
estaven a la part posterior feia que la tracció de les rodes fos correcta. Es va observar que la
velocitat del motor era extremadament alta. donat que els models de motor amb els que em
vaig proveir posseïen una reductora, que s´havia desmuntat anteriorment, es va decidir la
utilització d´aquesta, això va provocar que les rodes quedessin molt més avançades, i la
bateria a la part posterior feia que la tracció de les rodes amb el terra fos pràcticament nul·la.
La solució va ser en la creació de un nivell més a la part superior del robot per a la col·locació
de les bateries. Es va optar per a la reubicació de les bateries abans del total redisseny del
robot, ja que els terminis marcats estaven sobrepassats.
Un cop acabat el projecte puc dir que la solució que es va optar en aquell moment va ser la
més adient, però també esmentar que si tingués que fer un altre cop aquest projecte faria
molts canvis estructurals, com per exemple l’amplada del robot i també canviaria els motors
i engranatge utilitzats per un model de motor amb sortida de l´eix de forma lateral i equipat
amb codificador.
Treball Final de Grau Xavier Avila Busquets
62
6.4 Creació de plànols
Per al disseny del robot es va utilitzar la plataforma OpenSource Freecad. El programa
ofereix una gran varietat de funcions i és molt intuïtiu, però depèn de molta carrega del
processador del portàtil, això provocava que el ordinador s’alentís, i la realització de les
peces es fes pesada i repetitiva.
Aquest fet es va agreujar en la creació dels plànols. El programa no esta preparat per a la
realització de plànols de forma professional, per exemple, la inserció de les cotes de cada
peça es tenia que fer en el model 3D per a posteriorment afegir ’l en el model 2D del plànol.
Cada cop que es `passava del 3D al 2D hi havia números cap per avall o sobreposats.
Finalment la solució va estar en deixar la millor presentació possible tot hi quedar números
sobreposats o cap per avall.
La solució més idònia hauria sigut la creació de les peces mitjançant el programa Autocad,
el qual esta preparat per a aquest tipus de feina.
Treball Final de Grau Xavier Avila Busquets
63
7 Programa de control del robot
7.1 Matlab GUI
En aquest apartat es descriuràn les funcions principal que s´encarreguen de processar la
informació per a controlar el robot. L´estil per a crear el codi ha estat similar al que s´ha
realitzat durant tot el projecte, repartint les diferents tasques en Scripts o funcions per a poder
ser reutilitzades.
El programa consta d´una part principal que dóna funcionalitat a la interfície del programa.
Així com el que ha de fer quan es polsa cada un dels polsadors o quan es tanca el mateix
programa. A més d´aquestes funcions, també s´han creat d´altres per tal de processar les
dades que s´envien al robot. Es a dir, totes aquelles que calculen la potencia a subministrar
a cada roda. Tant sols mostrarem parts del programa ja que és mot extens.
El sistema esta basat en les coordenades en el gràfic que es mostra en la GUI. La velocitat i
angle de gir del robot, estarà determinat per la altura en el eix de les y, i la posició en x. Amb
les quals es procedirà al còmput de la velocitat per al còmput de la potencia a subministrar a
cada roda.
if x>=0 && y~=0 y=-y; else y=y; end R=y+x; L=y-x;
if R>255 R=255; else if R<-255 R=-255; end end
if L>255 L=255; else if L<-255 L=-255; end end
Per consultar la resta del codi, vegeu a l´apartat annexes: “Funció per al control del motor”.
Es va veure que amb aquest mètode hi havia zones del gràfic en que es quedava limitat la
velocitat a les que es podia aplicar a cada roda, per això està integrat un cercle el qual indica
els límits als que es pot operar el robot. Per a fer el càlcul d´aquests límits es va tindre que
utilitzar les següents línies de codi:
if rad>255 if x>0 den=sqrt(1+(tan(ang))^2); num=255*tan(ang); y=num/den; x=sqrt((255^2)-(y^2)); else den=sqrt(1+(tan(ang))^2); num=255*tan(ang); y=-num/den;
Treball Final de Grau Xavier Avila Busquets
64
x=-sqrt((255^2)-(y^2)); end else x=x; y=y; end
Per consultar la resta del codi, vegeu a l´apartat annexes: “Funció per calcular els límits de
la zona d´operació del robot”.
Per a la utilització de el teclat es va tindre que fer us de les funcions de Matlab:
kbhit=waitforbuttonpress; %#ok<NASGU> kbhit=get(gcf,'currentcharacter');
Per consultar la resta del codi, vegeu a l´apartat annexes: “Funció per la lectura del teclat”.
Aquestes funcions permet tindre una espera passiva fins que no s´ha apretat una tecla, i un
cop s´ha fet ens retorna la tecla apretada en la variable kbhit.
Per al control mitjançant el ratolí, es va voler agafar el punt precís en què es selecciones en
la gràfica, aquest punt seria la selecció de velocitat i direcció del robot. Per a la seva
implementació es va tindre que traspolar els valors que ens retornava el ratolí fins a obtenir
els marges de la nostra gràfica, per posteriorment proporcionar als valors entre -255 i 255:
if get (handles.mouse_select,'backgroundcolor')== [0 1 0 ] pos = get(hObject, 'currentpoint'); % get mouse location on figure x = pos(2); y = pos(1); % assign locations to x and y
x=(x-32)*17; if x<-255 x=-255; else if x>255 x=255; end end
y=(y-135)*6; if y<-255 y=-255; else if y>255 y=255; end end set(handles.mouse_x, 'string', x); % update text for x loc set(handles.mouse_y, 'string', y); % update text for y loc end
Per consultar la resta del codi, vegeu a l´apartat annexes: “Programa principal de la GUI”.
Finalment cal esmentar que tots els valors que s´utilitzaven per al control del motor eren
agafats a les finestres d´informació per al usuari. D´aquesta manera es podia fer una millor
implementació de tots els programes.
Treball Final de Grau Xavier Avila Busquets
65
7.2 Interfície amb el usuari
Per a la creació de una interfície gràfica per al usuari en el ordinador s´ha utilitzat l´eina de
Matlab GUI..
La GUI, o també coneguda com a interfície gràfica d´usuari, permet un control senzill
d´aplicacions de software, el qual elimina les necessitats d´aprendre un llenguatge i escriure
comandaments a fi d´executar una aplicació.
Les apps de Matlab son programes autònoms de MATLAB amb un frontal gràfic d´usuari
que automatitza una tasca o càlcul. Generalment la GUI inclous controls com menús, barres
d´eines, botons i controls lliscants.
Els paràmetres de disseny de la interfície gràfica que s´han tingut en compte han sigut la
facilitat de control del robot i donar la informació necessària per al usuari en pantalla
En la figura superior es pot observar la interfície d´usuari en la seva construcció, on es pot
observar la disposició de tots els botons i informació que se’ns mostraria per pantalla
habilitat.
Figura 7.1 Captura de pantalla de la interfície gràfica en construcció
Treball Final de Grau Xavier Avila Busquets
66
En la següent captura podrem observar la pantalla per a la connexió via Bluetooth.
Es pot observar que disposem de 4 modes de control del robot, els quals corresponen al
funcionament mitjançant lliscadors, mitjançant el pulsament de butons en la GUI, per teclat
i finalment amb el ratolí.
Figura 7.3 Captura de pantalla de GUI desconectat Figura 7.2 Captura de pantalla de GUI conectat
Figura 7.5 Captura de pantalla de GUI amb control
per lliscador Figura 7.4 Captura de pantalla de GUI amb control
per botons
Figura 7.7 Captura de pantalla de GUI amb control
per teclat Figura 7.6 Captura de pantalla de GUI amb control
per ratolí
Treball Final de Grau Xavier Avila Busquets
67
7.3 Android
A més a més s´ha creat una aplicació per Android mitjançant la aplicació online del MIT
anomenada “Google App Inventor”.
Google App Inventor és un plataforma de Google Labs per crear aplicacions de software
operatiu Android. De forma visual y a partir de un conjunt d´eines bàsiques, es pot anar
enllaçant una sèrie de blocs per crear l´aplicació. Totalment gratuït i fàcilment descarregable.
Com en la versió per a Matlab s´han incorporat varis tipus de control per al dispositiu, com
de forma tàctil a la pantalla o la inclinació del dispositiu.
Figura 7.9 Captura de pantalla de Samsun S3
Mini del inici de l áplicació Figura 7.8 Captura de pantalla de Samsun S3
Mini de selecció de control
Treball Final de Grau Xavier Avila Busquets
68
Figura 7.11 Captura de pantalla de Samsun S3
Mini de connexió Bluetooth per a control tàctil Figura 7.10 Captura de pantalla de Samsun S3
Mini per a control tàctil
Figura 7.13 Captura de pantalla de Samsun S3
Mini de connexió Bluetooth per a control amb
accelerometre
Figura 7.12 Captura de pantalla de Samsun S3
Mini per a control amb accelerometre
Treball Final de Grau Xavier Avila Busquets
69
8 Conclusions finals del projecte La realització d´un projecte d’aquestes característiques suposa una certa disciplina,
metodologia, cerca d’informació, probes, errors, etc. Això dota a qui el realitza d’una
experiència enriquidora, nous coneixements capacitat per enfrontar-se a nous problemes i la
sensació d´estar millor preparat professionalment. És una experiència molt recomanable, que
uneix dos camps com son l’electrònica, la informàtica i el disseny.
El haver pogut desenvolupar un prototip funcional amb un cost força reduït és molt
satisfactori. Els resultats aconseguits animen a seguir evolucionant el robot, desenvolupant
aplicacions i implementant nous components intentant que el producte final sigui més fiable
i versàtil.
Treball Final de Grau Xavier Avila Busquets
70
9 Webgrafia
Suport Matlab i Arduino
http://es.mathworks.com/matlabcentral/fileexchange/32374-matlab-support-for-arduino--aka-
arduinoio-package-
Arduino
http://www.arduino.cc/
http://www.arduino.cc/en/Main/Software
Freecad
http://www.freecadweb.org/wiki/index.php?title=Download
Github
https://git-scm.com/downloads
Treball Final de Grau Xavier Avila Busquets
71
10 Annexes
10.1 Programes d ‘Arduino
10.1.1 Estudi del motor
10.1.1.1 Programa principal Arduino void setup()
//Initialitzations
ini_motorDC();
ini_lcd();
delay (1000);
// initialize interrupts
int_isrTimer(); //Interrupt Service Routine with Timer
int_encoder1(); //Interrupt Routine when pin encoder rising
//Initialize Serial Port comunications
ini_Serial();
// Grafic Interficie Display
lcd_program();
imprimir();
void loop()
if(comFinished)
for(j=1; j<x; j++)
strPwm += Trama[j];
PWM=strPwm.toInt(); // Convert String to Integer
imprimir();
strPwm="";
run_motorDC (PWM);
comFinished=false;
Serial.println(vel_sens);
10.1.1.2 Include.h // Pins motor1
#define in1 4 // Right rotary
#define in2 5 // Left rotary
#define enA 6 // Enable motor 1
//Pins encoder1
#define encA1 2
#define encB1 3
//Pin Joystick
#define pin_pad 3
//directions
#define right true
#define left false
//Pantalla LCD
#define I2C_ADDR 0x27 //A4 a SDA (Datos)
Treball Final de Grau Xavier Avila Busquets
72
//A5 a SCL (Clock)
#define BACKLIGHT_PIN 3
#define En_pin 2
#define Rw_pin 1
#define Rs_pin 0
#define D4_pin 4
#define D5_pin 5
#define D6_pin 6
#define D7_pin 7
//Time Interruption
#define ISR_Time 100000 // Interruption Time in microseconds
//Encoder specifications
#define ticks_per_rev 11
#define reduction_gear 34
10.1.1.3 Funcions control motor void run_motorDC (int vel)
if (vel<0)
digitalWrite (in1, LOW);
digitalWrite (in2, HIGH);
vel=-vel;
else
digitalWrite (in2, LOW);
digitalWrite (in1, HIGH);
analogWrite(enA,vel);
int GetVelocity()
int rpm;
int count= prev_countA-countA;
rpm=(60/((float)ISR_Time/1000000))*count/(ticks_per_rev*reduction_gear);
//Velocity calculation every Time Sample -->
//Ts=ISR_Time(microSeconds)/1000000=convert to seconds
//In a minute how many times repeat this Ts--> exemple for Ts=0,1s
60/0,1=600
//tick counts since last interrupt, when during Ts-->(count) actual_count
minus last count, then multiplicate for how much interrupts there are in
minute
//if i know how much counts there are in a minute, and i know how much
tocks there are in a revolution, tick counts is devided by ticks in a
revolution
prev_countA=countA;
return rpm;
Treball Final de Grau Xavier Avila Busquets
73
10.1.1.4 Inicialitzacions void ini_motorDC()
pinMode (enA, OUTPUT); // Enable outputs pins
pinMode (in1, OUTPUT);
pinMode (in2, OUTPUT);
void ini_Serial()
Serial.begin(9600); // Serial Port Comunications at 9600 Baud rate
strPwm.reserve(10); // Reserve 10 bytes
void ini_lcd()
lcd.begin (16,2); // Inicializar el display con 16 caraceres 2
lineas
lcd.setBacklightPin(BACKLIGHT_PIN,POSITIVE);
lcd.setBacklight(HIGH);
lcd.home (); // go home
lcd.print("Creat per Xurra");
lcd.setCursor ( 0, 1 ); // go to the 2nd line
lcd.print(" No-Linealitats");
void lcd_program()
lcd.home (); // go home
lcd.print("PWM= ");
lcd.setCursor ( 0, 1 ); // go to the 2nd line
lcd.print("RPM= ");
// Interruptions
void int_isrTimer()
Timer1.initialize(ISR_Time); // Time of interruption
Timer1.attachInterrupt( ISR_Callback) ; // Callback function
void int_encoder1()
attachInterrupt(digitalPinToInterrupt(encA1), encoderCountA1, RISING);
//Interrupt pin, ISR function, Mode
//attachInterrupt(digitalPinToInterrupt(encB1), encoderCountB1,
CHANGE);
10.1.1.5 Funcions d´interrupcio void serialEvent()
while(Serial.available())
data=(char)Serial.read();
if (data != 'z')
Trama[i++]=data;
Treball Final de Grau Xavier Avila Busquets
74
else
comFinished = true;
x=i;
i=0;
void encoderCountA1()
if ((digitalRead(encA1)) == HIGH) // found a low-to-high on channel A;
if(digitalRead(encoderPinA)==HIGH).... read PB0
if ((digitalRead(encB1)) == LOW) // check channel B to see which
way; if(digitalRead(encoderPinB)==LOW).... read PB0
countA++;
else
countA--;
void ISR_Callback()
detachInterrupt(digitalPinToInterrupt(encA1)); //Disable interrupt
when calculating
vel_sens= GetVelocity();
attachInterrupt(digitalPinToInterrupt(encA1), encoderCountA1, RISING);
//Interrupt pin, ISR function, Mode
10.1.1.6 Funcions de imprimir per pantalla LCD void imprimir()
// IMPRIMIR PWM
int print_PWM=(int)100*PWM/255;
lcd.setCursor (7, 0 );
if (print_PWM<100)
lcd.print(" ");
if (print_PWM<10)
lcd.print(" ");
lcd.print(print_PWM);
lcd.print(" % ");
// PRINT VELOCITY
Treball Final de Grau Xavier Avila Busquets
75
lcd.setCursor ( 7, 1 );
if (vel_sens<100)
lcd.print(" ");
if (vel_sens<10)
lcd.print(" ");
lcd.print(vel_sens);
lcd.print(" ");
Treball Final de Grau Xavier Avila Busquets
76
10.1.2 Control PID
10.1.2.1 Programa principal del PID //Llibreries
#include <Wire.h> //This library allows you to communicate
with I2C / TWI devices.
#include <LCD.h>
#include <LiquidCrystal_I2C.h> // This library allows an Arduino board
to control LiquidCrystal displays (LCDs) based on the Hitachi HD44780 (or
a compatible) chipset, which is found on most text-based LCDs. The
library works with in either 4- or 8-bit mode (i.e. using 4 or 8 data
lines in addition to the rs, enable, and, optionally, the rw control
lines).
#include <TimerOne.h> // This library allows comands to Timer
Interrupts
#include "inc.h" // This library allows all pins of
arduino
//Creació d´objectes
// LCD
LiquidCrystal_I2C
lcd(I2C_ADDR,En_pin,Rw_pin,Rs_pin,D4_pin,D5_pin,D6_pin,D7_pin);
//Control PID
struct pid_data
float kp, ki, kd;
float delta;
float sum;
float prevError;
;
//Variables globals
long countAR=0, countBR=0, prev_countAR=0;
long countAL=0, countBL=0, prev_countAL=0;
bool gir_R=true,gir_L=true;
int vel_sensR, vel_sensL;
int vel_refR, vel_refL;
// Array and Variables for communication
String strVel=""; //
boolean comFinished= false;
char Trama[5], data=0; // string for data recivied
int i=0, j=0, x=0; //"i" is Index for reception data, "j" is for save
data in internal variable, "x" is lenght of recivied data
int PWM_R,PWM_L;
struct pid_data pid_data_R, pid_data_L;
void setup()
//Initialitzations
ini_motorDC();
ini_lcd();
delay (1000);
// initialize interrupts
int_isrTimer(); //Interrupt Service Routine with Timer
int_encoder(); //Interrupt Routine when pin encoder rising
Treball Final de Grau Xavier Avila Busquets
77
//Initialize Serial Port comunications
ini_Serial();
// Grafic Interficie Display
lcd_program();
control_pid_ini(pid_data_R, 0.01, 4, 0.1, 0); //Delta, P estabilitat,
I=Temps de resposta, D=arribar a valor despres de variació brusca
control_pid_ini(pid_data_L, 0.01, 4, 0.1, 0); //Delta, P estabilitat,
I=Temps de resposta, D=arribar a valor despres de variació brusca
imprimir();
void loop()
char command, motor;
if(comFinished)
command=Trama[0];
switch (command)
case 'r':
for(j=1; j<x; j++) //Read until end "x" of frame
strVel += Trama[j];
vel_refR=strVel.toInt(); // Convert String to Integer
if (vel_refR<0)
gir_R=true;
vel_refR=-vel_refR;
else
gir_R=false;
strVel="";
break;
case 'l':
for(j=1; j<x; j++) //Read until end "x" of frame
strVel += Trama[j];
vel_refL=strVel.toInt(); // Convert String to Integer
if (vel_refL<0)
gir_L=true;
vel_refL=-vel_refL;
else
gir_L=false;
strVel="";
break;
case 'v':
motor=Trama[1];
if (motor=='r')
Treball Final de Grau Xavier Avila Busquets
78
Serial.println(vel_sensR);
else if (motor=='l')
Serial.println(vel_sensL);
break;
default:
Serial.println("Error comunicacion");
break;
comFinished=false;
imprimir();
10.1.2.2 Include.h // Pins motor1
#define in1 8 // Right rotary
#define in2 7 // Left rotary
#define enA 5 // Enable motor 1 Left
#define in3 9 // Right rotary
#define in4 10 // Left rotary
#define enB 6 // Enable motor 2 Right
//Pins encoder1
#define encAR 2
#define encBR 11
#define encAL 3
#define encBL 12
//directions
#define right true
#define left false
//Pantalla LCD
#define I2C_ADDR 0x27 //A4 a SDA (Datos)
//A5 a SCL (Clock)
#define BACKLIGHT_PIN 3
#define En_pin 2
#define Rw_pin 1
#define Rs_pin 0
#define D4_pin 4
#define D5_pin 5
#define D6_pin 6
#define D7_pin 7
//Time Interruption
#define ISR_Time 100000 // Interruption Time in microseconds
//Encoder specifications
#define ticks_per_rev 11
#define reduction_gear 34
//Maximum velocity and PWM
#define MAX_PWM 255
Treball Final de Grau Xavier Avila Busquets
79
#define MAX_VELOCITY 130
10.1.2.3 Funcions void run_motorR (int vel)
if (vel<0)
digitalWrite (in3, LOW);
digitalWrite (in4, HIGH);
vel=-vel;
else
digitalWrite (in4, LOW);
digitalWrite (in3, HIGH);
analogWrite(enB,vel);
void run_motorL (int vel)
if (vel<0)
digitalWrite (in1, LOW);
digitalWrite (in2, HIGH);
vel=-vel;
else
digitalWrite (in2, LOW);
digitalWrite (in1, HIGH);
analogWrite(enA,vel);
int GetVelocityR()
int rpm;
int count= countAR-prev_countAR;
rpm=(60/((float)ISR_Time/1000000))*count/(ticks_per_rev*reduction_gear);
//Velocity calculation every Time Sample -->
//Ts=ISR_Time(microSeconds)/1000000=convert to seconds
//In a
minute how many times repeat this Ts--> exemple for Ts=0,1s 60/0,1=600
//tick
counts since last interrupt, when during Ts-->(count) actual_count minus
last count, then multiplicate for how much interrupts there are in
minute
//if i know
how much counts there are in a minute, and i know how much tocks there
are in a revolution, tick counts is devided by ticks in a revolution
prev_countAR=countAR;
return rpm;
int GetVelocityL()
Treball Final de Grau Xavier Avila Busquets
80
int rpm;
int count= countAL-prev_countAL;
rpm=(60/((float)ISR_Time/1000000))*count/(ticks_per_rev*reduction_gear);
//Velocity calculation every Time Sample -->
//Ts=ISR_Time(microSeconds)/1000000=convert to seconds
//In a
minute how many times repeat this Ts--> exemple for Ts=0,1s 60/0,1=600
//tick
counts since last interrupt, when during Ts-->(count) actual_count minus
last count, then multiplicate for how much interrupts there are in
minute
//if i know
how much counts there are in a minute, and i know how much tocks there
are in a revolution, tick counts is devided by ticks in a revolution
prev_countAL=countAL;
return rpm;
10.1.2.4 Inicialitzacións void ini_motorDC()
//Right Motor
pinMode (enA, OUTPUT); // Enable outputs pins
pinMode (in1, OUTPUT);
pinMode (in2, OUTPUT);
//Left Motor
pinMode (enB, OUTPUT); // Enable outputs pins
pinMode (in3, OUTPUT);
pinMode (in4, OUTPUT);
void ini_Serial()
Serial.begin(9600); // Serial Port Comunications at 9600 Baud rate
strVel.reserve(10); // Reserve 10 bytes
void ini_lcd()
lcd.begin (16,2); // Inicializar el display con 16 caraceres 2
lineas
lcd.setBacklightPin(BACKLIGHT_PIN,POSITIVE);
lcd.setBacklight(HIGH);
lcd.home (); // go home
lcd.print("Creat per Xurra");
lcd.setCursor ( 0, 1 ); // go to the 2nd line
lcd.print("2 Motors control PID");
void lcd_program()
lcd.home (); // go home
lcd.print("PWMr= l= ");
lcd.setCursor ( 0, 1 ); // go to the 2nd line
lcd.print("RPMr= l= ");
Treball Final de Grau Xavier Avila Busquets
81
// Interruptions
void int_isrTimer()
Timer1.initialize(ISR_Time); // Time of interruption
Timer1.attachInterrupt( ISR_Callback) ; // Callback function
void int_encoder()
attachInterrupt(digitalPinToInterrupt(encAR), encoderCountAR, RISING);
//Interrupt pin, ISR function, Mode
attachInterrupt(digitalPinToInterrupt(encAL), encoderCountAL, RISING);
10.1.2.5 Funcions d´interrupcó void serialEvent()
while(Serial.available())
data=(char)Serial.read();
if (data != 'z')
Trama[i++]=data;
else
comFinished = true;
x=i;
i=0;
void encoderCountAR()
countAR++;
void encoderCountAL()
countAL++;
void ISR_Callback()
detachInterrupt(digitalPinToInterrupt(encAR)); //Disable interrupt
when calculating
detachInterrupt(digitalPinToInterrupt(encAL));
//Right
vel_sensR= GetVelocityR();
float error = (vel_refR - vel_sensR);
PWM_R=(int) Get_pid(pid_data_R, error);
if (gir_R) PWM_R=-PWM_R;
Treball Final de Grau Xavier Avila Busquets
82
run_motorR (PWM_R);
//Left
vel_sensL= GetVelocityL();
error = vel_refL - vel_sensL;
PWM_L=(int) Get_pid(pid_data_L, error);
if (gir_L) PWM_L=-PWM_L;
run_motorL (PWM_L);
attachInterrupt(digitalPinToInterrupt(encAR), encoderCountAR, RISING);
//Interrupt pin, ISR function, Mode
attachInterrupt(digitalPinToInterrupt(encAL), encoderCountAL, RISING);
10.1.2.6 Funcions del PID void control_pid_ini(struct pid_data &pid, float delta, float kp, float
ki, float kd)
pid.delta = delta;
pid.kp = kp;
pid.ki = ki;
pid.kd = kd;
pid.sum = 0;
pid.prevError = 0;
float Get_pid(struct pid_data &pid, float error)
//Calcul de P
float p = pid.kp * error;
//Calcul de I
pid.sum += error;
float i = pid.ki * pid.delta * pid.sum;
//Calcul de D
float d = pid.kd * (error - pid.prevError) / pid.delta;
pid.prevError = error;
float _pid=p+i+d;
//Limitació de valors
if(_pid<0) _pid=0;
if(_pid>255) _pid=255;
return _pid;
10.1.2.7 Funcions de impressió per pantalla LCD void imprimir()
// IMPRIMIR PWM
int print_PWM=(int)100*PWM_R/MAX_PWM;
if (print_PWM<0)
lcd.setCursor (4,0);
Treball Final de Grau Xavier Avila Busquets
83
if(print_PWM>-100)
lcd.print(" ");
if(print_PWM>-10)
lcd.print(" ");
else
lcd.setCursor (5, 0 );
if (print_PWM<100)
lcd.print(" ");
if (print_PWM<10)
lcd.print(" ");
lcd.print(print_PWM);
lcd.print("%");
print_PWM=(int)100*PWM_L/MAX_PWM;
if (print_PWM<0)
lcd.setCursor (11,0);
if(print_PWM>-100)
lcd.print(" ");
if(print_PWM>-10)
lcd.print(" ");
else
lcd.setCursor (12, 0 );
if (print_PWM<100)
lcd.print(" ");
if (print_PWM<10)
lcd.print(" ");
lcd.print(print_PWM);
lcd.print("%");
// PRINT VELOCITY
//Right
lcd.setCursor ( 5, 1 );
if (vel_sensR<100)
lcd.print(" ");
if (vel_sensR<10)
Treball Final de Grau Xavier Avila Busquets
84
lcd.print(" ");
lcd.print(vel_sensR);
lcd.print(" ");
//Left
lcd.setCursor ( 12, 1 );
if (vel_sensL<100)
lcd.print(" ");
if (vel_sensL<10)
lcd.print(" ");
lcd.print(vel_sensL);
lcd.print(" ");
10.1.3 Codi configuració Bluetooth #include <SoftwareSerial.h>
SoftwareSerial BT1(4,2); // RX, TX
void setup()
Serial.begin(9600);
Serial.println("Enter AT commands:");
BT1.begin(9600);
void loop()
if (BT1.available())
Serial.write(BT1.read());
if (Serial.available())
String S = GetLine();
BT1.print(S);
Serial.println("---> " + S);
String GetLine()
String S = "" ;
if (Serial.available())
char c = Serial.read(); ;
while ( c != '\n') //Hasta que
el character sea intro
S = S + c ;
delay(25) ;
c = Serial.read();
return( S + '\n') ;
10.1.4 Codi per a linealització de Velocitat #include "pins.h"
Treball Final de Grau Xavier Avila Busquets
85
long mA1, mA2, mB1, mB2;
long up_times_A[3]=0,0,0,up_times_B[3]=0,0,0;
int indexA = 0, indexB = 0, i=0, j;
float speed = 0;
int pwm = 0;
char dataComing= 0, dataDisplay;
bool motorAenabled = false, motorBenabled = false, firstTime = true;
float timeBetweenPulses; // (us/count)
//
void encMotorA()// Interrupt Motor A
mA1 = micros();
up_times_A[indexA]=mA1-mA2;
indexA=(indexA+1)%3;
mA2 = mA1;
void encMotorB()// Interrupt Motor B
mB1 = micros();
up_times_B[indexB]=mB1-mB2;
indexB=(indexB+1)%3;
mB2 = mB1;
void setup()
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
// Initialize outputs and inputs
MYSERIAL.begin(BAUDRATE);
pinMode(INTERRUPT_PIN_MOTOR_A,INPUT);
digitalWrite(INTERRUPT_PIN_MOTOR_A,HIGH);
pinMode(INTERRUPT_PIN_MOTOR_B,INPUT);
digitalWrite(INTERRUPT_PIN_MOTOR_B,HIGH);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN_MOTOR_A),encMotorA,RI
SING);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN_MOTOR_B),encMotorB,RI
SING);
analogWrite(ENA,pwm);
analogWrite(ENB,pwm);
// Print information in serial port
MYSERIAL.println("This program computes the velocity of a motor,");
MYSERIAL.println("the program will print 100 values of velocities");
MYSERIAL.println();
MYSERIAL.println("Type any character to begin the program: ");
while(MYSERIAL.read()<0); // Wait until a character will be type
void loop()
//
if(firstTime) // Choose if you are gonna use motor A or B
Treball Final de Grau Xavier Avila Busquets
86
MYSERIAL.print("Type A or B for taking data from one of the motors:
");
firstTime = false;
switch (dataDisplay) // Switch Motor A or B
case 'A':
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,LOW);
motorAenabled = true;
MYSERIAL.print(" MOTOR A ENABLED ");
break;
case 'B':
digitalWrite(IN1,LOW);
digitalWrite(IN2,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
motorBenabled = true;
MYSERIAL.print(" MOTOR B ENABLED ");
break;
if(dataDisplay > 0)
MYSERIAL.println(dataDisplay);
dataComing = 0;
dataDisplay = 0;
if(motorAenabled && !motorBenabled)
analogWrite(ENA,pwm);
timeBetweenPulses = up_times_A[0] + up_times_A[1] + up_times_A[2];
// timeBetweenPulses = timeBetweenPulses/3;
else if (!motorAenabled && motorBenabled)
analogWrite(ENB,pwm);
delay(100);
timeBetweenPulses = up_times_B[0] + up_times_B[1] + up_times_B[2];
timeBetweenPulses = timeBetweenPulses/3;
if(motorAenabled || motorBenabled)
if (i <= 300) // Plot 260 velocity values
delay(100);
speed=MIN2MICROSECONDS/(COUNTS_PER_REVOLUTION*timeBetweenPulses);
// Compute speed in rpm
if(timeBetweenPulses != 0)
if(speed <= 200) MYSERIAL.println(speed); // Print speed in rpm
else MYSERIAL.println(0);
else
MYSERIAL.println(0);
//MYSERIAL.println(pwm); // Print speed in rpm
//timeBetweenPulses = 0;
if(pwm < 255) pwm = pwm + 1;
i++;
Treball Final de Grau Xavier Avila Busquets
87
//
void serialEvent () // Serial Event to receive characters from serial
port
while(MYSERIAL.available())
dataComing=(char)MYSERIAL.read();
if(dataComing != '\n')
dataDisplay = dataComing;
//
10.1.5 Codi per a trobar els valors del motor #include "pins.h"
long mA1, mA2, mB1, mB2;
long up_times_A[3]=0,0,0,up_times_B[3]=0,0,0;
int indexA = 0, indexB = 0, i=0, j, count = 0;
float speed = 0;
int pwm = 245;
char dataComing= 0, dataDisplay;
bool motorAenabled = false, motorBenabled = false, firstTime = true;
float timeBetweenPulses; // (us/count)
void encMotorA()// Interrupt Motor A
mA1 = micros();
up_times_A[indexA]=mA1-mA2;
indexA=(indexA+1)%3;
mA2 = mA1;
void encMotorB()// Interrupt Motor B
mB1 = micros();
up_times_B[indexB]=mB1-mB2;
indexB=(indexB+1)%3;
mB2 = mB1;
void setup()
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
// Initialize outputs and inputs
MYSERIAL.begin(BAUDRATE);
pinMode(INTERRUPT_PIN_MOTOR_A,INPUT);
digitalWrite(INTERRUPT_PIN_MOTOR_A,HIGH);
pinMode(INTERRUPT_PIN_MOTOR_B,INPUT);
digitalWrite(INTERRUPT_PIN_MOTOR_B,HIGH);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN_MOTOR_A),encMotorA,RI
SING);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN_MOTOR_B),encMotorB,RI
SING);
analogWrite(ENA,0);
analogWrite(ENB,0);
// Print information in serial port
Treball Final de Grau Xavier Avila Busquets
88
MYSERIAL.println("This program computes the velocity of a motor,");
MYSERIAL.println("the program will print 100 values of velocities");
MYSERIAL.println();
MYSERIAL.println("Type any character to begin the program: ");
while(MYSERIAL.read()<0); // Wait until a character will be type
void loop()
if(firstTime) // Choose if you are gonna use motor A or B
MYSERIAL.print("Type A or B for taking data from one of the motors:
");
firstTime = false;
switch (dataDisplay) // Switch Motor A or B
case 'A':
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,LOW);
motorAenabled = true;
MYSERIAL.print(" MOTOR A ENABLED ");
analogWrite(ENA,pwm);
delay(2000);
break;
case 'B':
digitalWrite(IN1,LOW);
digitalWrite(IN2,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
motorBenabled = true;
MYSERIAL.print(" MOTOR B ENABLED ");
analogWrite(ENB,pwm);
delay(2000);
break;
if(dataDisplay > 0)
MYSERIAL.println(dataDisplay);
dataComing = 0;
dataDisplay = 0;
if(motorAenabled && !motorBenabled)
analogWrite(ENA,pwm);
timeBetweenPulses = up_times_A[0] + up_times_A[1] + up_times_A[2];
timeBetweenPulses = timeBetweenPulses/3;
else if (!motorAenabled && motorBenabled)
analogWrite(ENB,pwm);
timeBetweenPulses = up_times_B[0] + up_times_B[1] + up_times_B[2];
timeBetweenPulses = timeBetweenPulses/3;
if(motorAenabled || motorBenabled)
if(j < 600)
if(count < 100)
speed=MIN2MICROSECONDS/(COUNTS_PER_REVOLUTION*timeBetweenPulses); //
Compute speed in rpm
Treball Final de Grau Xavier Avila Busquets
89
MYSERIAL.println(speed);
count++;
//MYSERIAL.println(count);
delay(100);
else
if(pwm == 160) pwm = 245;
else pwm = 160;
// MYSERIAL.println(pwm);
j--;
count = 0;
j++;
//MYSERIAL.println(j);
else
pwm = 0;
void serialEvent () // Serial Event to receive characters from serial
port
while(MYSERIAL.available())
dataComing=(char)MYSERIAL.read();
if(dataComing != '\n')
dataDisplay = dataComing;
Treball Final de Grau Xavier Avila Busquets
90
10.2 Programes de Matlab
10.2.1 Estudis del motor
10.2.1.1 Script per al estudi de No-Linealitats function play_calculate_linelity_test()
clc close all clear all
[serial_port]=Ini_Port(); [array] =lineality_test (0,serial_port); fplot_lineality_test (array);
fclose(serial_port); delete(serial_port); end
10.2.1.2 Script per a configuració de comunicació mitjançant Port USB function [s]=Ini_Port()
% initialize Port Communication
s = serial('COM4');
set(s, 'BaudRate', 9600); set(s, 'InputBufferSize', 512); %number of bytes in inout buffer set(s, 'FlowControl', 'none'); set(s, 'Parity', 'none'); set(s, 'DataBits', 8); set(s, 'StopBit', 1); set(s, 'Timeout',10);
fopen(s); pause (5);
end
10.2.1.3 Script per a la recollida de dades de No-Linealitats function [array] =lineality_test (number,s) close all;
N=255; % Maximum os PWM in LN298 driver Ts=10/255; % ten seconds of slope divided by 255 steps ( Per
tenir una pujada al valor maxim en 10 segons) Fs=1/Ts; % Sample Frequency in seconds^(-1) Max_Velocity=234;
rpm = zeros (uint16 (50/Ts),1); %RPM array pwm = zeros (uint16 (50/Ts),1); %PWM array
tic %start time sample count while (number<=N) % increase slope from 0 to 100% if toc > 1/Fs % enter every Ts tic % Restart time sample count
Treball Final de Grau Xavier Avila Busquets
91
str = int2str(number); % convert number to string to
send send = ['a' str 'z']; % save string to send with SOF
and EOF
fwrite(s,send,'char','sync'); % Send Frame
rpm(1:end-1)=rpm(2:end); %Move all string one
pisition back rpm(end)=(fscanf(s,'%d'))/Max_Velocity; % Read velocity
and save on the last position of array
pwm(1:end-1)=pwm(2:end); %Move all string one back pisition pwm(end)=(number/255); %Save number in the last position
of string
number=number+1; %Increase PWM end end
number=255; % when out of last increase the value of pwm is
256. tic while (0<=number) % decrease slope 100 to 0% if toc > 1/Fs tic % Restart time sample count
str = int2str(number); % convert number to string to
send send = ['a' str 'z']; % save string to send with SOF
and EOF
fwrite(s,send,'char','sync'); % Send Frame
rpm(1:end-1)=rpm(2:end); %Move all string one
pisition back rpm(end)=(fscanf(s,'%d'))/Max_Velocity; % Read velocity
and save on the last position of array
pwm(1:end-1)=pwm(2:end); %Move all string one back pisition pwm(end)=(number/255); %Save number in the last position
of string
number=number-1; end end
number=0; espera=0; tic while (espera<=5) %wait 5 seconds at 0 if toc > 1/Fs espera=espera+toc; % added the time to wait 5 seconds in
while tic % Restart time sample count
str = int2str(number); % convert number to string to
send
Treball Final de Grau Xavier Avila Busquets
92
send = ['a' str 'z']; % save string to send with SOF
and EOF
fwrite(s,send,'char','sync'); % Send Frame
rpm(1:end-1)=rpm(2:end); %Move all string one
pisition back rpm(end)=(fscanf(s,'%d'))/Max_Velocity; % Read velocity
and save on the last position of array
pwm(1:end-1)=pwm(2:end); %Move all string one back pisition pwm(end)=(number/255); %Save number in the last position
of string end end
tic while (-N<=number) % decrease slope from 0 to -100% if toc > 1/Fs tic % Restart time sample count
str = int2str(number); % convert number to string to
send send = ['a' str 'z']; % save string to send with SOF
and EOF
fwrite(s,send,'char','sync'); % Send Frame
rpm(1:end-1)=rpm(2:end); %Move all string one
pisition back rpm(end)=(fscanf(s,'%d'))/Max_Velocity; % Read velocity
and save on the last position of array
pwm(1:end-1)=pwm(2:end); %Move all string one back pisition pwm(end)=(number/255); %Save number in the last position
of string
number=number-1; end end
number=-255; tic while (0>=number) % increase slope from -100 to 0% if toc > 1/Fs tic % Restart time sample count
str = int2str(number); % convert number to string to
send send = ['a' str 'z']; % save string to send with SOF
and EOF
fwrite(s,send,'char','sync'); % Send Frame
rpm(1:end-1)=rpm(2:end); %Move all string one
pisition back rpm(end)=(fscanf(s,'%d'))/Max_Velocity; % Read velocity
and save on the last position of array
Treball Final de Grau Xavier Avila Busquets
93
pwm(1:end-1)=pwm(2:end); %Move all string one back pisition pwm(end)=(number/255); %Save number in the last position
of string
number=number+1; end end
number=0; espera=0; tic while (espera<5) %wait 5 seconds at 0 if toc > 1/Fs espera=espera+toc; tic % Restart time sample count
str = int2str(number); % convert number to string to
send send = ['a' str 'z']; % save string to send with SOF
and EOF
fwrite(s,send,'char','sync'); % Send Frame
rpm(1:end-1)=rpm(2:end); %Move all string one
pisition back rpm(end)=(fscanf(s,'%d'))/Max_Velocity; % Read velocity
and save on the last position of array
pwm(1:end-1)=pwm(2:end); %Move all string one back pisition pwm(end)=(number/255); %Save number in the last position
of string end end
data(:,1)=rpm(:,1); data(:,2)=pwm(:,1); array=timeseries(data); % Create a timeseries of a
vector array.TimeInfo.Increment=Ts; %Define interval of timseries
Vector end
10.2.1.4 Script per a la realització del gràfic del estudi de No-Linealitats
function fplot_lineality_test (arr)
plot (arr); title ('Gràfica de no-linealitats'); xlabel ('Temps (Segons)'); ylabel ('%Velocitat i PWM'); legend ('Velocitat (%)', 'PWM (%)');
end
10.2.1.5 Script per a l´execució del estudi del ample de banda function [Ft,No_offset_signals,array_rpm] = play_Ancho_de_banda()
Treball Final de Grau Xavier Avila Busquets
94
clc close all clear all
%Creació de la senyal d´entrada [array_pwm,T,Ts,time]= input_Ancho_de_banda ();
%Creació del objecte de comunicació [serial_port]=Ini_Port();
%Obtenció de dades [array_rpm, array_pwm] = Ancho_de_banda_run (serial_port,array_pwm,
T,Ts);
%Tancament de comunicació fclose(serial_port); delete(serial_port);
%Tractament de la senyal [No_offset_signals] = Tractament_de_senyal (array_rpm,array_pwm);
%Anàlisis de dades i plots [Ft] = FT_Bode (No_offset_signals); Fast_Function_Transfer (Ts,No_offset_signals); fplot_Ancho_de_banda(array_rpm, array_pwm,time,No_offset_signals,Ft); end
10.2.1.6 Script de creacio de senyal sinusoïdal de freqüència variable function [Data_sinestream,T,Ts,time]= input_Ancho_de_banda ()
Ts=0.01;
input_arr= frest.createFixedTsSinestream(Ts,0.6, 4*2*pi); % (Ts,sys, w) input_arr.Amplitude = 0.2; input_arr.NumPeriods = 4;
%extreiem els punts de la señal input input= generateTimeseries(input_arr); Data_sinestream=input.data(:,1)+0.5; %Obtenim els punts de la señal i
afegim un offset per treballar a la zona de no linealitats
T= (length (input.data))*Ts; % Temps d´execució de la señal time=0:Ts:T-0.01; end
10.2.1.7 Script per introducció al sistema la senyal sinusoïdal function [rpm, pwm] = Ancho_de_banda_run (s,input, T, Ts)
close all; Fs=1/Ts; % Sample Frequency in seconds^(-1)
rpm= zeros ((length (input)), 1); %RPM array
Treball Final de Grau Xavier Avila Busquets
95
pwm=input; %PWM array espera=0; i=1; tic while (i<=length (input))%(espera<=T) %wait until finished if toc > 1/Fs espera=espera+toc; tic % Restart time sample count number=pwm(i)*255;
str = int2str(number); % convert number to string to
send send = ['a' str 'z']; % save string to send with
SOF and EOF fwrite(s,send,'char','sync'); % Send Frame
rpm(i)=fscanf(s,'%d')/208; % Read velocity and save
on the last position of array
pwm(i)=(number/255); %Save number in the last position
of string i=i+1; end end
str = int2str(0); % convert number to string to send send = ['a' str 'z']; % save string to send with SOF and EOF
fwrite(s,send,'char','sync'); % Send Frame
end
10.2.1.8 Tractament de senyal de sortida del ample de banda function [signals] = Tractament_de_senyal (rpm,pwm)
rpm_no_offset=rpm; Data_no_offset=pwm; for i=0:1:10; rpm_no_offset(1,:)=[]; %Esborrar els primers 10 valors Data_no_offset(1,:)=[]; end
rpm_no_offset=detrend(rpm_no_offset,'constant'); %Treure offset de la
senyal de rpm Data_no_offset=detrend(Data_no_offset,'constant'); %Treure offset de la
senyal de rpm
signals=Data_no_offset; signals(:,2)=rpm_no_offset;
end
10.2.1.9 Script de la creació de Fast Function Transfer and plot function Fast_Function_Transfer (Ts,signals)
rpm_no_offset=signals(:,2); %Nomes agafo rpm
Treball Final de Grau Xavier Avila Busquets
96
L= length (rpm_no_offset); Fs=1/Ts;
NFFT=2^nextpow2(L); Y=fft(rpm_no_offset,NFFT)/L; f=Fs/2*linspace(0,1,NFFT/2+1);
figure plot (f,2*abs(Y(1:NFFT/2+1))); grid title ('Single-Sided amplitude Spectrum of y(t)'); xlabel ('Frequency (Hz)'); ylabel('|Y(f)|'); end
10.2.1.10 Script de creació de funció de transferència del sistema function [Ft] = FT_Bode (signals) Ft = tfest(signals,1); end
10.2.1.11 Gràfiques de totes les senyals del estudi de ample de banda function fplot_Ancho_de_banda (rpm,pwm,time,signals,Ft)
%Gràfica de entrada y salida juntas figure
plot (time,rpm,time,pwm); %Primera prova ha tardat 25 minuts T*5 title ('Gràfica de Resposta ample de banda'); xlabel ('Temps (Segons)'); ylabel ('%Velocitat i PWM'); legend ('Velocitat (%)', 'PWM (%)');
%Gràfica de entrada y sortide per separat figure subplot(2,1,1); plot (time,rpm); title ('RPM'); xlabel ('Temps (s)'); ylabel ('Velocitat (rpm)');
subplot(2,1,2); plot (time,pwm,'r'); title ('PWM'); xlabel ('Temps (s)'); ylabel ('PWM (%)');
cut_time=time;
for i=0:1:10; cut_time(:,1)=[]; %Esborrar els primers 10 valors end %Gràfica de senyal de sortida sense Offset rpm_no_offset=signals(:,2); figure plot (cut_time,rpm_no_offset); grid title ('Senyal de velocitat sense offset');
Treball Final de Grau Xavier Avila Busquets
97
xlabel('Temps (s)'); ylabel('Velocitat');
%Gràfica del Bode figure bode(Ft); grid
end
10.2.1.12 Script per a la linealització de la velocitat load dades.mat close all y_rpm = vector(101:300); y_pwmModel_cut = vector(101:300);
pwm = zeros(200,1); for i = 1:156 pwm(i) = 99 + i; end pwm(157:end)=255; %% p = polyfit(pwm,y_rpm,2); pwm_model = zeros(300,1); pwm_model(1) = 0; pwm_model(1:255,1) = 1:255; pwm_model(256:end,1) = 255; y_pwmModel = zeros(length(pwm_model),1); for i = 1:length(pwm_model) y_pwmModel(i) = p(1)*pwm_model(i)^2 +p(2)*pwm_model(i)+p(3); end
plot(pwm,y_rpm) hold on for i= 1:200 y_pwmModel_cut(i)=y_pwmModel(i+99); end plot(pwm,y_pwmModel_cut,'r') title ('Gràfica de linealització de la velocitat'); xlabel ('Temps (Segons)'); ylabel ('Velocitat'); legend('Motor','Linealitzat');
10.2.1.13 Script de per l´obtenció dels paràmetres del motor close all clear all global y u t; load motorDreta.mat
y = motorD(201:600,1); a = 1;
%%
u=zeros(length(y),1); a = 1;
Treball Final de Grau Xavier Avila Busquets
98
for j=1:(length(y)/100) switch a case 1 for i=1+(j-1)*100:100+(j-1)*100 u(i)=p(1)*245^2+p(2)*245+p(3); % pasem 245 pwm a rpm end a = 2; case 2 for i=1+(j-1)*100:100+(j-1)*100 u(i)=p(1)*165^2+p(2)*165+p(3); end a = 1; end end t = zeros(length(y),1);
for i=2:(length(y)) t(i) = t(i-1) + 0.1; end %% J = 1; K = 1; b = 1; L = 1; R=1; params = [J b K R L]; newParams = fminsearch(@distance,params) %% J = newParams(1); b = newParams(2); K = newParams(3); R = newParams(4); L = newParams(5);
A=[-b/J K/J; -K/L R/L] B=[0; 1/L]; C=[1,0]; D=0;
sys_model=ss(A,B,C,D); [y_model,t] = lsim(sys_model,u,t); plot(t,y_model,'r'); hold on plot(t,y); title ('Gràfica de parametres estimats'); xlabel ('Temps (Segons)'); ylabel ('Velocitat'); legend('y_m_o_d_e_l','y');
10.2.1.14 Script per a l´aproximació de valors function r = distance(params) % (J) moment of inertia of the rotor % (b) motor viscous friction constant % (K) electromotive force constant and motor torque constant % (R) electric resistance % (L) electric inductance J=params(1); b=params(2); K=params(3); R=params(4); L=params(5); A=[-b/J K/J; -K/L R/L]
Treball Final de Grau Xavier Avila Busquets
99
B=[0; 1/L]; C=[1,0]; D=0; eigenValues = eig(A); sys=ss(A,B,C,D); %Now we discretize the systsem with the same %rate the data was provided h=0.1; if ((real(eigenValues(1))<=0) && (real(eigenValues(2))<=0) &&(R>=0) &&
(b>=0) && (J>=0)) sysd=c2d(sys,h); %And now we simulate the response global u %system input, recorded with arduino global t %timming of the input global y %Real output recorded with arduino [y_model,t]=lsim(sysd,u,t); r=norm(y-y_model); else r = inf; end end
10.3 Comandes AT per Bluetooth HC-06
AT+VERSION , per conèixer la versió del Firmware del dispositiu
AT+NAMEXXX, Programar el nom del dispositiu
AT+BAUDX, Fixa la velocitat de comunicació entre el modul i la consola:
1 configura 1200bps
2 configura 2400bps
3 configura 4800bps
4 configura 9600bps
5 configura 19200bps
6 configura 38400bps
7 configura 57600bps
8 configura 115200bps
AT+PINXXXX, configura el número d’identificació personal
Treball Final de Grau Xavier Avila Busquets
100
10.4 Programa de control del robot
10.4.1 Programa principal de la GUI function varargout = GUI1(varargin) % GUI1 MATLAB code for GUI1.fig % GUI1, by itself, creates a new GUI1 or raises the existing % singleton*. % % H = GUI1 returns the handle to a new GUI1 or the handle to % the existing singleton*. % % GUI1('CALLBACK',hObject,eventData,handles,...) calls the local % function named CALLBACK in GUI1.M with the given input arguments. % % GUI1('Property','Value',...) creates a new GUI1 or raises the % existing singleton*. Starting from the left, property value pairs
are % applied to the GUI before GUI1_OpeningFcn gets called. An % unrecognized property name or invalid value makes property
application % stop. All inputs are passed to GUI1_OpeningFcn via varargin. % % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only
one % instance to run (singleton)". % % See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help GUI1
% Last Modified by GUIDE v2.5 22-Mar-2016 17:18:16
% Begin initialization code - DO NOT EDIT gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @GUI1_OpeningFcn, ... 'gui_OutputFcn', @GUI1_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []); if nargin && ischar(varargin1) gui_State.gui_Callback = str2func(varargin1); end
if nargout [varargout1:nargout] = gui_mainfcn(gui_State, varargin:); else gui_mainfcn(gui_State, varargin:); end % End initialization code - DO NOT EDIT
% --- Executes just before GUI1 is made visible. function GUI1_OpeningFcn(hObject, eventdata, handles, varargin) % This function has no output args, see OutputFcn. % hObject handle to figure % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA)
Treball Final de Grau Xavier Avila Busquets
101
% varargin command line arguments to GUI1 (see VARARGIN)
%while don´t connected don´t show Pad, either graphic axes set(handles.Pad,'Visible','off'); set(handles.axes1,'visible','off'); set(handles.ConnectButton,'String','Disconnected');
%Set to 0 variables (Globals) set(handles.y_info,'String','0'); set(handles.x_info,'String','0');
% Choose default command line output for GUI1 handles.output = hObject;
% Update handles structure guidata(hObject, handles);
% UIWAIT makes GUI1 wait for user response (see UIRESUME) % uiwait(handles.figure1);
% --- Outputs from this function are returned to the command line. function varargout = GUI1_OutputFcn(hObject, eventdata, handles) % varargout cell array for returning output args (see VARARGOUT); % hObject handle to figure % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA)
% Get default command line output from handles structure varargout1 = handles.output;
% --- Executes on button press in up. function up_Callback(hObject, eventdata, handles) % hObject handle to up (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % x_axes=x_axes+1; % if x_axes > 255 % x_axes=255; % end % set (handles.x_info,'String',x_axes);
% --- Executes on button press in left. function left_Callback(hObject, eventdata, handles) % hObject handle to left (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % y_axes=y_axes-1; % if y_axes < -255 % y_axes=-255; % end % set (handles.y_info,'String',y_axes);
% --- Executes on button press in right.
Treball Final de Grau Xavier Avila Busquets
102
function right_Callback(hObject, eventdata, handles) % hObject handle to right (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % y_axes=y_axes+1; % if y_axes > 255 % y_axes=255; % end % set (handles.y_info,'String',y_axes);
% --- Executes on button press in down. function down_Callback(hObject, eventdata, handles) % hObject handle to down (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % x_axes=x_axes-1; % if x_axes < -255 % x_axes=-255; % end % set (handles.x_info,'String',x_axes);
function ConnectButton_Callback(hObject, eventdata, handles)
str=get(hObject,'String'); if str == 'Disconnected' set(handles.Pad,'Visible','on'); set(handles.axes1,'visible','on'); set(handles.ConnectButton,'String','Connected'); else if str == 'Connected' set(handles.Pad,'Visible','off'); set(handles.axes1,'visible','off'); set(handles.ConnectButton,'String','Disconnected'); else msgbox('Connection Error','COMPORT') end end
10.4.2 Inicialització del port function[s]= Ini_Port ()
% initialize Port Communication %Connect via USB %s = serial('COM4', 'BaudRate', 9600);
%Connect via Bluetooth s = Bluetooth('XURRA_BT',1);
%s='Falta conectar' pause (1); end
Treball Final de Grau Xavier Avila Busquets
103
10.4.3 Funció per al control del motor function MC (x,y,s)
if x>=0 && y~=0 y=-y; else y=y; end R=y+x; L=y-x;
if R>255 R=255; else if R<-255 R=-255; end end
if L>255 L=255; else if L<-255 L=-255; end end
R=R L=L str = int2str(R); send = ['r' str 'z']; fwrite(s,send,'char','sync'); pause (0.1);
str = int2str(L); send = ['l' str 'z']; fwrite(s,send,'char','sync');
end
10.4.4 Funció per la lectura del teclat function [val] = keyboard_in ()%s)
close all; kbhit=1; while kbhit ~= '0' kbhit=waitforbuttonpress; %#ok<NASGU> kbhit=get(gcf,'currentcharacter'); switch kbhit case 30 %Forward str = int2str(180); % convert number to string to
send command='a'; val='Endavant' case 31 %Backward str = int2str(0); command='f'; val='Endarrera' case 29 %right str = int2str(180);
Treball Final de Grau Xavier Avila Busquets
104
command='r'; val='dreta' case 28 %left str = int2str(180); command='l'; val='esquerra' case '0' str=int2str(90); command='S'; otherwise val='Error' end send = [command str 'z']; fwrite(s,send,'char','sync'); end
end
10.4.5 Impressió de la gràfica function print (x,y) p=plot ([y 0], [x 0]); set (p,'linewidth',2); set (p,'MarkerSize',5); set (p,'Marker','o'); if x<0 x=-x; end if y<0 y=-y; end ylim([-255 255]); xlim([-255 255]); grid
hold on circle(0,0,255); hold off end
function circle(x,y,r) %x and y are the coordinates of the center of the circle %r is the radius of the circle %0.01 is the angle step, bigger values will draw the circle faster but %you might notice imperfections (not very smooth) ang=0:0.01:2*pi; xp=r*cos(ang); yp=r*sin(ang); plot(x+xp,y+yp,'k');
end
Treball Final de Grau Xavier Avila Busquets
105
10.4.6 Funció per calcular els límits de la zona d´operació del robot function [x,y]=limites(x,y)
if x==0 && y==0 rad=0; ang=0; else rad=sqrt((x^2)+(y^2)); ang= (atan(y/x)); end
if rad>255 if x>0 den=sqrt(1+(tan(ang))^2); num=255*tan(ang); y=num/den; x=sqrt((255^2)-(y^2)); else den=sqrt(1+(tan(ang))^2); num=255*tan(ang); y=-num/den; x=-sqrt((255^2)-(y^2)); end else x=x; y=y; end
Treball Final de Grau Xavier Avila Busquets
114
