Desarrollo de un Equipo de Control de un Motor de ...bibing.us.es/proyectos/abreproy/91081/fichero/memoria+MPG.pdf · 3 Montaje y Prueba del Funcionamiento de los Motores 11 ... 3Desarrollo

i

i

Desarrollo de un Equipo de Control de un Motor de Corriente Continua basado en Arduino

Equation Chapter 1 Section 1

Trabajo Fin de Grado

Grado en Ingeniería de las Tecnologías

Industriales

Desarrollo de un Equipo de Control de un Motor de

Corriente Continua basado en Arduino

Dep. de Ingeniería de Sistemas y Automática

Escuela Técnica Superior de Ingeniería

Universidad de Sevilla

Autor: Marta Peláez Gallardo

Tutor: Ignacio Alvarado Aldea

Sevilla, 2017

iii

Proyecto Fin de Grado

Grado en Ingeniería de las Tecnología Industriales

Desarrollo de un Equipo de Control de un Motor de

Corriente Continua basado en Arduino

Autor:

Marta Peláez Gallardo

Tutor:

Ignacio Alvarado Aldea

Profesor Contratado Doctor

Dep. de Ingeniería de Sistemas y Automática

Escuela Técnica Superior de Ingeniería

Universidad de Sevilla

Sevilla, 2017

v

Proyecto Fin de grado: Desarrollo de un Equipo de Control de un Motor de Corriente Continua basado en

Arduino

Autor: Marta Peláez Gallardo

Tutor: Ignacio Alvarado Aldea

El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros:

Presidente:

Vocales:

Secretario:

Acuerdan otorgarle la calificación de:

Sevilla, 2017

El Secretario del Tribunal

vii

A mi familia

ix

Agradecimientos

Han sido 6 años los que me ha llevado sacar esta carrera, 6 años, con sus más y sus menos, con muchos

momentos de sufrimiento y esfuerzo, pero de mucha satisfacción cuando las cosas salían correctamente. Pero

no ha sido un camino que haya recorrido sola, muchas personas han vivido esto conmigo y en gran parte este

éxito es gracias a ellos.

A mis padres y hermanos, quienes han aguantado a la mejor y a la peor Marta durante todos estos años, y que

siempre han estado apoyándome y brindándome todo su cariño.

A las personas que ya no están a mi lado, pero que han crecido conmigo y me han acompañado hasta llegar a

ser quien soy y a conseguir lo que he conseguido.

Y por supuesto a mi tutor Ignacio, quien me ha ayudado a que este proyecto salga adelante, gracias por todo.

Marta Peláez Gallardo

Sevilla, 2017

xi

Resumen

En nuestra Escuela se realizan multitud de prácticas para afianzar los conocimientos teóricos que se imparten

en las distintas asignaturas. Este proyecto será usado para realizar una serie de prácticas en el ámbito del

estudio de las distintas estrategias de control en varios dispositivos de bajo coste por el Departamento de

Ingeniería de Sistemas y Automática.

xv

Índice

Agradecimientos ix

Resumen xi

Índice xv

Índice de Tablas xvii

Índice de Figuras xix

1 Objeto del Proyecto 1

2 Materiales de Partida y Características 2 2.1 Soporte 2 2.2 Motores 3 2.3 Ruedas 4 2.4 Encoders de motores 4 2.5 Unidad Microcontroladora: ARDUINO UNO(ATmega328P) 5 2.6 Placa Shield Ardumoto 7 2.7 Placa de prueba 8

3 Montaje y Prueba del Funcionamiento de los Motores 11 3.1 Interrupciones Temporales 11

3.1.1 Descripción para el uso de los Timers 11 3.1.2 Tipos de Timers en la placa Arduino Uno 11 3.1.3 Asociación de Timers y PWM 11 3.1.4 Tipos de registros de un Timer 12 3.1.5 Configuración del preescalador y del modo de operación 13 3.1.6 Inicialización Timer 1 14 3.1.7 Rutina de Interrupción de Timer 1 16

4 Montaje de los Encoders 17 4.1 Interrupción de cambio de pin 18

5 Obtención de la velocidad y la aceleración 20 5.1 Velocidad 20 5.2 Característica estática 21 5.3 Zona muerta y compensador 22 5.4 Signo de la velocidad 22 5.5 Aceleración 23

6 Control en Velocidad 24 6.1 PI en velocidad 25

7 Control en Aceleración 30

Apéndice de Código 33

xvii

ÍNDICE DE TABLAS

Tabla 2-1. Descripción pines Ardumoto 8

Tabla 4-1. Pines de Interrupción. 18

Tabla 5-1. Zona muerta. 22

Tabla 6-1. Tabla Ziegler-Nichols. 26

Tabla 6-2. Características que se ven afectadas al ajustar el controlador. 27

xix

ÍNDICE DE FIGURAS

Figura 2-1. Soporte. 2

Figura 2-2. Simulación soporte. 2

Figura 2-3. Dimensiones del motor. 3

Figura 2-4. Motor. 4

Figura 2-5. Rueda. 4

Figura 2-6. Encoders. 5

Figura 2-7. Arduino uno. 6

Figura 2-8. Escudo Ardumoto. 7

Figura 2-9. Esquema general de conexiones. 9

Figura 3-1. Conexión placa-motor. 11

Figura 3-2. Registro TCCR1A. 12

Figura 3-3. Registro TCCR1B. 12

Figura 3-4. Selección del preescalador. 13

Figura 3-5. Configuración del modo de operación. 14

Figura 3-6. Código de inicialización del Timer 1. 15

Figura 3-7. Rutina Interrupción Timer 1. 16

Figura 4-1. Función de interrupción. 17

Figura 4-2. Activar Interrupción. 18

Figura 4-3. Función de Interrupción. 18

Figura 4-4. Función de interrupción externa. 18

Figura 4-5. Bibliotecas Interrupciones cambio de pin. 19

Figura 4-6. Ejemplo Interrupción cambio de pin. 19

Figura 5-1. Obtención de la velocidad. 20

Figura 5-2. Función de Matlab filtro Butterworth. 21

Figura 5-3. Velocidad filtrada. 21

Figura 5-4. Característica estática. 22

Figura 5-5. Calculo de la aceleración. 23

Figura 6-1. Diagrama de bloques PI. 24

Figura 6-2. Sentido anti horario. 24

Figura 6-3. Sentido horario. 24

Figura 6-4. Control PI en velocidad. 28

Figura 6-5. Control PI en velocidad(rev/s). 28

Figura 6-6. Cambio de referencia positiva a negativa(rev/s). 29

Figura 7-1. Obtencion PI en aceleración. 31

Figura 7-2. Control PI en aceleración. 31

Figura 7-3. Gestión del signo de la velocidad. 32

xxi

1

1 OBJETO DEL PROYECTO

l objetivo de este proyecto es el desarrollo de un equipo de prácticas de control de bajo coste. Este equipo de

prácticas consta de un motor de corriente continua con un encoder, para medir la velocidad y un arduino uno

como unidad de control en tiempo real.

E

2 MATERIALES DE PARTIDA Y

CARACTERÍSTICAS

El hardware que se expone en esta memoria se ha realizado desde cero.

El material con el que se ha contado consta de:

2.1 Soporte

Se trata de una estructura de montaje y apoyo para el resto de elementos. Permite la estabilidad y el mejor

manejo del conjunto de elementos, ya que evita que estos se separen o se dañen.

Se ha diseñado con Fusion360 y reproducido usando una impresora 3D.

Figura 2-2. Simulación soporte.

Figura 2-1. Soporte.

3

3


2.2 Motores

Se trata de dos motores de corriente continua de la marca DAGU Hi-Tech Electronic Co, con denominación

DG02S Mini DC Gear Motor, que funciona con una tensión nominal de 3V a 6V. El motor tiene incorporada

una caja reductora de velocidad con una reducción de 48:1 y unos encoders con sensores de tipo efecto hall

que permiten conocer la velocidad de giro del motor.

Figura 2-3. Dimensiones del motor.

Las principales características de este motor son:

Voltaje: 3V - 6V

Sin corriente de la carga: 200mA

Corriente de parada: 3A @ 6V (1.5A @ 3V)

Relación de caja de cambios: 48: 1

Velocidad de la rueda: 65rpm @ 3V descargado (Recomendado)

Materiales de Partida y Características

4

Figura 2-4. Motor.

2.3 Ruedas

Se trata de una rueda de 65mm de diámetro, con neumático de caucho negro. El buje consiste en un taladro

ciego para el eje con un tornillo prisionero con cabeza allen para fijar el conjunto eje rueda.

Figura 2-5. Rueda.

2.4 Encoders de motores

Los encoders de DAGU, son un simple complemento para cualquier robot con ruedas que pueden ayudar a

medir la velocidad o la distancia a la que el chasis viaja. Cada encoder de la rueda consta de un disco que tiene

4 imanes de neodimio, con protección de goma y un sensor de efecto Hall terminado con cables de 150 mm y

cabezales de servo de 3 pines hembra. Estos codificadores de rueda requieren una tensión de alimentación de

3-24V con una corriente de alimentación de 4mA.

5

5


Figura 2-6. Encoders.

Características:

Tensión de alimentación: 3-24V

Corriente de suministro: 4mA por sensor

Voltaje de salida: 26V Max

Corriente de salida: 25mA

Salida de encoder: drenaje abierto con protección contra cortocircuitos.

2.5 Unidad Microcontroladora: ARDUINO UNO(ATmega328P)

Arduino es una plataforma de hardware libre, basada en una placa con un microcontrolador y un entorno

de desarrollo, diseñada para facilitar el uso de la electrónica en proyectos multidisciplinares.

El hardware consiste en una placa con un microcontrolador y puertos entrada/salida. El software consiste

en un entorno de desarrollo que implementa el lenguaje de programación y el cargador de arranque que es

ejecutado en la placa. El entorno de desarrollo integrado libre se puede descargar gratuitamente.

La unidad microcontroladora será la que se encargue de recibir todos los datos disponibles de los sensores

y realizar los cálculos para decidir la señal de control que tiene que enviar a los motores, para así mantener

el equilibrio.


6

Características generales:

Microcontrolador: Atmega380

Tensión operativa: 5V

Tensión de entrada(recomendada): 7-12V

Tensión de entrada (limites): 6-20V

Pines de entrada/salida digitales: 14(de los que 6 proporcionan salida PWM)

Pines de entrada analógica: 6

Corriente continua por Pin E/S: 40mA

Corriente continua para el Pin 3.3V: 50mA

Memoria Flash: 32KB de los que 0.5KB son usados por el bootloader

SRAM: 2KB

EEPROM: 1KB

Frecuencia de reloj: 16MHz

Figura 2-7. Arduino uno.

Entradas y salidas digitales: Están situadas en la parte de arriba de la placa, van del 0 hasta el 13.

La señal digital puede estar a nivel bajo a 0V y nivel alto a 5V. (LOW o HIGH).

Salidas PWM: Son los pines 11, 10, 9, 6, 5 y 3, marcados con el carácter ~. Se denominan señales PWM (Pulse Width Modulation) a un tipo de señal de voltaje utilizada para enviar

información o para modificar la cantidad de energía que se envía a una carga. Este tipo de señal es muy

utilizada en circuitos digitales que necesitan emular una señal analógica.

Son de tipo cuadrada o sinusoidales en las cuales se le cambia el ancho relativo respecto al período

de la misma, llamado ciclo de trabajo (Duty cicle).

Entradas analógicas: Son los pines A0, A1, A2, A3, A4 y A5 (analog in). Transforman una señal

de 0 a 5V en un número que va de 0 a 1023. (porque usa un conversor analógico-digital de 10bits).

7

7


Pines de alimentación:

GND: Son los pines a tierra de la placa, el negativo.

5v: Por este pin suministra 5v

3,3v: Por este pin suministra 3,3v

Vin: Voltaje de entrada, por este pin también se puede alimentar la placa.

RESET: Por este pin se puede reiniciar la placa

IOREF: Sirve para que la placa reconozca el tipo de alimentación que requieren los shields

También podemos encontrar el pin AREF, este pin sirve para variar la referencia del voltaje,

diferente a 5v, y que así el conversor analógico-digital funcione a otra tensión. Se utiliza la función

analogReference().

También están el conector USB, para cargar el programa y alimentar la placa; y el conector de

alimentación, para alimentarla.

2.6 Placa Shield Ardumoto

El escudo Ardumoto es un controlador de dos motores.

Aquí está una vista anotada del escudo, destacando los pines y los componentes importantes:

Figura 2-8. Escudo Ardumoto.

https://cdn.sparkfun.com/assets/b/8/6/c/2/527c0c05757b7fc33c8b4567.png?__hstc=43953530.b31804002ddec0ffb5c2ec615681a0f0.1483882021260.1483882021260.1483961202962.2&__hssc=43953530.1.1483961202962&__hsfp=1962957139


8

El escudo Ardumoto requiere cuatro pines de Arduino para su control: 3, 11, 12 y 13. Cada motor

utiliza dos pines - uno para la dirección, el otro controla la velocidad.

Tabla 2-1. Descripción pines Ardumoto

Pin de

Arduino

Etiqueta del Pin del

escudo de Ardumoto Notas

3 PWM A Una señal PWM para controlar la velocidad del motor A. 0

= off, 255 = velocidad máxima.

11 PWM B Una señal PWM para controlar la velocidad del motor B. 0

= off, 255 = velocidad máxima.

12 DIR A Una señal digital para controlar el sentido de rotación del

motor A (por ejemplo, HIGH / LOW => CW / CCW).

13 DIR B Una señal digital para controlar el sentido de rotación del

motor B (por ejemplo, HIGH / LOW => CW / CCW).

Mientras el escudo Ardumoto está unido a un Arduino, estos pines no deben estar conectados a otra

cosa.

Al lado de cada una de las salidas del motor hay un par de LEDs azules y amarillos, que indican la

dirección de giro de su motor.

2.7 Placa de prueba

Como parte del proceso final del proyecto, se decidió reducir todo el cableado al mínimo. Esto es posible

diseñando y fabricando una placa de prueba que incluya la conexión a los encoders de los motores.

Esta placa es de tipo shield, esto es, va “enchufada” directamente sobre la unidad Arduino.

A continuación, en la figura, se incluye el esquema general de conexiones usando una resistencia de 10KΩ:

9

9


Figura 2-9. Esquema general de conexiones.

3 MONTAJE Y PRUEBA DEL FUNCIONAMIENTO

DE LOS MOTORES

e conectan los motores a la placa tipo shield Ardumoto. Para este proyecto usaremos únicamente uno de

los motores, el B.

Figura 3-1. Conexión placa-motor.

Para comprobar el correcto funcionamiento de los motores se aplica el código que podemos ver en el apéndice

Código 1.1.

Este código, tras definir el sentido de giro y los motores, asigna los pines correspondientes a la señal PWM y a

la dirección. Los pines serán inicializados como salidas a nivel bajo.

Tras esto utiliza la función “driveArdumoto (motor, dir, spd)” que recibe el motor que queremos que se

mueva, el sentido de giro y la velocidad a la que queremos que se mueva dicho motor; y la función

“stopArdumoto(motor)” que se encargara de su parada. Se realizan una serie de pruebas.

Para el control de los motores, necesitamos ejecutar el código de control en tiempos concretos (tiempo de

muestreo), para ello usaremos las interrupciones temporales.

3.1 Interrupciones Temporales

Cuando el programa requiere que se ejecute parte de su código en instantes concretos utilizaremos

interrupciones temporales. Mediante estas interrupciones temporales, pararemos la ejecución del programa

s

11

11 Desarrollo de un Equipo de Control de un Motor de Corriente Continua basado en Arduino

principal el tiempo necesario para ejecutar un determinado código (lo más breve posible) en un instante

concreto, para luego seguir con la ejecución del programa principal donde se dejó. Los instantes en que se

ejecutan estas interrupciones se controlan con temporizadores.

3.1.1 Descripción para el uso de los Timers

Un Timer es un temporizador que se puede configurar para que en determinado tiempo dispare una

interrupción. Para el correcto funcionamiento del Timer se tienen que configurar algunos registros, ver sección

3.1.4, y en su inicialización se debe configurar al menos el modo de operación. El preescalador, es un divisor

de frecuencia configurable.

Las interrupciones temporales son funciones que se activan una vez el Timer haya dado la orden, es decir, el

Timer es un contador y cuando el valor del registro que va aumentando a lo largo del tiempo desborda o llega

a un valor concreto, como en nuestro caso, se dispara para activar el código de la interrupción y esto lo hará

cada cierto tiempo (tiempo fijo).

3.1.2 Tipos de Timers en la placa Arduino Uno

La placa de Arduino de la que disponemos tiene el microcontrolador ATmega328P, este está compuesto por 3

Timers distintos entre ellos:

Timer 0: es un temporizador de 8 bits, es decir, que registrará 28 valores que son 256. Este

temporizador es usado en las funciones delay(), millis() y micros(), por lo que se debe tener en cuenta

si se usa o no cuando se programa. Este Timer se puede configurar en modo contador (el valor del

registro TMR0 se incrementa con cada ciclo de instrucción) o en modo temporizador (el valor del

registro TMR0 se incrementa en cada flanco de reloj, que puede ser ascendente o descendente).

Timer 1: es un temporizador de 16 bits, este puede registrar hasta 1024 valores. Este temporizador se

usa en la librería servo, pero dado que en este proyecto esta librería no se usa, se puede hacer uso de

este temporizador. El Timer 1 tiene dos registros de 8 bits que son para lectura y escritura. A

diferencia del Timer 0 este temporizador puede activar o detener la cuenta. El Timer 1 también se

puede configurar en modo temporizador o en modo contador al igual que el temporizador 0.

Timer 2: es un temporizador de 8 bits. Es similar al Timer 0 con la diferencia de que este es el que se

usa para la función tone(). Este temporizador cuenta con un preescalador y un post-escalador. Este

timer ya está en uso para el PWM de uno de los motores con el que se consigue configurar la

frecuencia, como veremos en el siguiente apartado. Por consiguiente, no es posible usarlo para otra

tarea distinta como puede ser la realización en un intervalo de tiempo fijo de la acción de control.

3.1.3 Asociación de Timers y PWM

Las funciones PWM por hardware emplean los Timers para generar la onda de salida. Cada Timer da servicio

a 2 o 3 salidas PWM.

Cada salida conectada a un mismo temporizador comparte la misma frecuencia, aunque pueden tener distintos

ciclos de trabajo (Duty cicle), dependiendo del valor de su registro de comparación.

En el caso de Arduino Uno:

El Timer0 controla las salidas PWM 5 y 6.

Montaje y Prueba del Funcionamiento de los Motores

12



En nuestro caso, como definimos en la tabla del apartado 2.6., únicamente usaremos las señales PWM 3 y 11;

y como dijimos en el apartado anterior, por esto, el timer 2 está ya en uso.

3.1.4 Tipos de registros de un Timer

Para cambiar el comportamiento del Timer tendremos que configurar los registros del mismo. Los más

importantes son los siguientes:

TCCRx: registro de control de la cuenta del temporizador (Timer/Counter Control Register). En este

registro se configura el preescalador y el modo de operación. Se divide entre el A y el B en cada uno

de los Timers.

Figura 3-2. Registro TCCR1A.

Figura 3-3. Registro TCCR1B.

En las figuras se utiliza de ejemplo el Timer 1 cuyo registro es TCCR1, este registro tiene a su vez dos,

TCCR1A y TCCR1B, según otras tablas que aparecen más adelante se puede definir como se ha mencionado

antes el preescalador a través de los valores de CSxy. Para el modo de operación se usarán WGMxy cuyos

registros se configuran según valores de otra tabla que aparece más adelante.

TCNTx: registro contador (Timer/Counter Register). En este registro se almacena el valor actual

del Timer.

OCRx: registro de comparación a la salida (Output Compare Register), este es un modo de

operación que se explicará más adelante.

ICRx: registro de captura a la entrada (Input Capture Register), este solo es válido para Timers de

16 bits. Es un registro usado para uno de los tipos de los modos de operación al igual que el

registro OCRx.

TIMSKx: registro de la máscara de las interrupciones del contador. Este activa o desactiva las

interrupciones del Timer.

TIFRx: registro de la bandera de las interrupciones del contador. Este indica las interrupciones

pendientes del Timer.

13


3.1.5 Configuración del preescalador y del modo de operación

Los Timers dependen de una fuente de reloj del propio microcontrolador, por lo tanto, la unidad más

pequeña medible es el periodo que ofrece el reloj del microcontrolador. Para saber dicho periodo se sabe

que este reloj es de 16 MHz, por lo que calculando la inversa se obtiene el periodo.

La fórmula para el cálculo del periodo con el dato de la frecuencia es la siguiente:

𝑇 =1

𝑓=

1

16 𝑀𝐻𝑧= 62.5 𝑛𝑠

Buscamos que las interrupciones sean cada 10 ms para el control PI. Por lo tanto, el tiempo fijo para que

se active la interrupción es mucho mayor que el periodo calculado del reloj del microcontrolador. Esto

quiere decir que los Timers se deben configurar con los requisitos especificados de tiempo y no con los

configurados por defecto (los del reloj interno).

Inicialmente se configura el preescalador según los valores CSx0, CSx1 y CSx2. El preescalador es un

divisor de frecuencia configurable antes de cada incremento. Su finalidad es hacer que el temporizador sea

más lento comparado con el reloj interno de la placa microcontroladora.

El cálculo del preescalador aparece en el apartado 3.1.5. de cálculo de los valores de los registros para la

inicialización de los Timers. Se observa en la Figura 3-4 qué valores tiene cada registro según el

preescalador requerido, se utiliza en esta tabla el Timer 1 como ejemplo.

Figura 3-4. Selección del preescalador.

A continuación, se explican los diferentes modos de operación que se pueden usar, existen cuatro modos:

Timer overflow (desbordar): cuando el temporizador alcanza un valor límite, el valor del registro

TOVx se establece en el registro de la interrupción por bandera que es el registro TIFRx, se

llamará a la rutina de interrupción (interrupt service routine) ISR(TIMERx_OVF_vect).

Output Compare Match (activa la interrupción por comparación de valores): cuando salta este tipo

de interrupción, la bandera OCFxy del registro OCTx se establece en el registro de la interrupción

por bandera, TIFRx. A continuación, se establece el output compare match interrupt service en el

registro TIMSKx a través de la activación de OCIExy, que permite que la interrupción esté

activada. Esta rutina se llama ISR(TIMERx_COMPy_vect).

Timer Input Capture (captura el tiempo especificado por el programador para activar la

interrupción): si salta este tipo de interrupción se activará TIFRx ya que se activa ICFx y TIMSKx

hará que comience la rutina de interrupción ISR(TIMERx_CAPT_vect).


14

PWM (modulación por ancho de pulsos): este modo modifica el ciclo de trabajo de una señal

periódica.

Para elegir el modo de operación se configuran unos valores determinados en los registros, estos se

pueden ver en la Figura 3-5 donde también se usa el Timer 1 como ejemplo.

Figura 3-5. Configuración del modo de operación.

El modo de operación que se va a utilizar es el Output Compare, modo CTC, mirando la Figura 3-5 se

comprueba que este modo es el cuarto, por lo que en la inicialización tendremos que activar o desactivar

WGMx0, WGMx1, WGMx2 y WGMx3. Por otra parte, según esta tabla se sabe que en el registro

OCR1A u OCR5A (según el Timer que se necesite usar) hay que guardar el valor máximo hasta el que se

tiene que contar para llegar al tiempo especificado, en nuestro caso a 10.

3.1.6 Inicialización Timer 1

Lo primero que se debe hacer es configurar el Timer sabiendo que la interrupción debe producirse cada 10 ms

(control PI). Es conocido que la frecuencia de la fuente de reloj es de 16 MHz. Teniendo en cuenta que el

periodo que se busca es 10 ms, es decir una frecuencia de 100 Hz se sabe que habrá que modificar los valores

por defecto del reloj ya existente en la placa de Arduino.

Primeramente, se comienza cambiando el preescalador para intentar conseguir los 10 ms de periodo, teniendo

en cuenta que el Timer 1 es de 16 bits. Si no se encontraran se necesita hacer uso del valor CTC (valor

máximo que se debe contar del modo de operación Output Compare, para configurar el periodo deseado), si se

llega a este valor se activa la interrupción.

Se sabe que este puede contar hasta el valor 65536, por lo que el valor de CTC que se calcula para que pasen

los 10 ms debe ser menor que el valor máximo que puede contar este Timer. Para que esto se cumpla se elige

el preescalador de 8 cuyo periodo será el siguiente:

𝑇 =1

16𝑀𝐻𝑧8⁄

= 5 ∙ 10−4𝑚𝑠

15


Para configurar este preescalador se hace uso de la tabla mostrada en la Figura 3-9 donde se comprueba que

CS11 debe valer uno y CS10 y CS12 valdrán cero.

Seguidamente se configura el modo de operación donde se debe definir el valor hasta el que se debe contar

para conseguir que la interrupción se active cada 10 ms. Antes de esta configuración se observa que la

interrupción se activaría cada 33 ms solo con el preescalador tal y como se comprueba en la ecuación:

𝑇 = (216 − 1) ∙ (5 ∙ 10−4𝑚𝑠) = 32.7675 𝑚𝑠 ≈ 33 𝑚𝑠

Por lo tanto, se calcula el valor CTC para corregir esto ya que el tiempo deseado no son 33 ms sino 10 ms:

𝑉𝑎𝑙𝑜𝑟 𝐶𝑇𝐶 =𝑡𝑖𝑒𝑚𝑝𝑜 𝑑𝑒𝑠𝑒𝑎𝑑𝑜

𝑟𝑒𝑠𝑜𝑙𝑢𝑐𝑖ó𝑛 𝑇𝑖𝑚𝑒𝑟− 1 =

10𝑚𝑠

5 ∙ 10−4𝑚𝑠− 1 = 19999

Para configurar este valor se almacenará el valor de CTC en el registro OCR1A. Para este Timer CTC será

19999.

Se debe tener en cuenta que esta inicialización se hará en el setup() del código, ya que es donde se inicializan

las variables, registros, librerías, etc. Esta función solo se ejecutará una vez, bien porque se ha encendido la

placa o bien porque se resetea y vuelve a inicializarse. En la Figura 3-13 se muestra el código de inicialización

del Timer 1 que estará situado en el setup().

void setup()

noInterrupts(); //desactiva todas las interrupciones para inicializar la interrupción temporal

TCCR1A = 0; //se inicializan los registros de control del Timer 1

TCCR1B = 0;

TCNT1 = 0;

OCR1A =19999; // valor CTC que se guarda en el registro OCR1A

TCCR1B |= (1 << WGM12); //configuración del modo output compare

TCCR1B |= (1 << CS11); //configuración del preescalador de 8

TIMSK1 |= (1 << OCIE1A); // activa la interrupción por comparación temporal

interrupts(); // activa todas las interrupciones

Figura 3-6. Código de inicialización del Timer 1.


16

3.1.7 Rutina de Interrupción de Timer 1

La rutina ISR del servicio de interrupciones de comparación de resultados, debe nombrarse de la siguiente

forma: ISR(TIMERx_COMPy_vect).

Donde el sufijo x representa el número de temporizador y el sufijo y el número de salida.

En esta rutina se programará el código correspondiente a la activación de una bandera, con la que llevaremos a

cabo el resto de acciones.

ISR(TIMER1_COMPA_vect) // rutina de interrupción con Output Compare

flag=1;

Figura 3-7. Rutina Interrupción Timer 1.

Las rutinas de interrupción han de ser lo más breves posibles

Han de utilizar variables de tipo “volatile”. Estrictamente hablando, no es un tipo de variable, sino una

directiva para el compilador. Eso significa que la variable en cuestión, debe ser almacenado de un cierto modo

que evite algunos problemas raros o conflictos que pueden surgir cuando una variable puede ser cambiada por

las interrupciones de servicio(ISR) o por el programa.

Hay determinadas funciones que no se pueden usar dentro de esta rutina, ya que usan otras interrupciones. Es

el caso de funciones como:

delay (), millis () y micros()

Serial.print(), Serial.println(), Serial.write() y Serial.read()

Debe evitarse el uso de las funciones digitalRead () y digitalWrite (), ya que tienen un impacto serio sobre la

velocidad y como hemos dicho anteriormente la función debe ser corta y rápida.

Además, ISR no pueden devolver parámetros ni tampoco recibirlos, pero si variables globales.

4 MONTAJE DE LOS ENCODERS

os motores disponen de Encoders o codificadores de ángulo. Se trata de interruptores q son sensibles a

los campos magnéticos (esto es un sensor de efecto hall) en nuestro caso (también podrían ser ópticos,

como en los ratones), que se utilizan para medir la velocidad de giro del motor, porque solidariamente

con el eje gira un disco que tiene 4 imanes que abren y cierran el interruptor del enconder con su paso. Como

son 4 imanes tendremos 8 polos, lo que provocará que el interruptor cambie de abierto a cerrado y viceversa 8

veces por vuelta del eje del motor.

En nuestro chasis creamos un espacio para colocar el sensor de efecto Hall agarrado con una brida. Y el disco

de 8 polos se introduce en el eje del motor con la zona metálica mirando hacia el sensor. El sensor de efecto

Hall debe estar horizontal al disco.

Mediante los encoders somos capaces medir el número de flancos que se generan en la etapa de movimiento.

Al tratarse de un disco con 4 imanes, es decir 8 polos, una vuelta equivaldrá a 4 pulsos, 4 flancos de subida y 4

de bajada. Y con cada flanco incrementan un contador, este se dividirá entre 8 para así obtener un contador de

vueltas y no de flancos. A partir de este dato determinaremos la velocidad de giro del motor y usando la

equivalencia de la reductora, en nuestro caso 48:1, podemos obtener la velocidad de giro de la rueda.

La lectura de los encoders se realiza, utilizando las interrupciones temporales descritas anteriormente, cada

10ms mediante la activación de la bandera.

La función que usaremos para la interrupción será:

attachInterrupt();

Figura 4-1. Función de interrupción.

Y dicha interrupción puede ser de varios tipos:

LOW: Activa cuando V del pin es 0.

HIGH: Actica cuando V del pin es Vmax.

CHANGE: Activa cuando el pin cambia de HIGH → LOW o LOW → HIGH.

RISING: Activa cuando el pin cambia de LOW → HIGH.

FALLING: Activa cuando el pin cambia de HIGH → LOW.

Usaremos el siguiente código para activar la interrupción del encoder:

#define encoderB 2

void setup()

// tras la activación de las interrupciones temporales

Serial.begin(9600);

pinMode(encoderB,INPUT_PULLUP); // deshabilita la resistencia pullup interna

attachInterrupt (0,doencoderB,CHANGE); //encoder B en interrupción 0 (pin2)

L

Montaje de los Encoders

18

Figura 4-2. Activar Interrupción.

La función de interrupción de encoder se encarga de incrementar un contador con cada paso de HIGH →

LOW o LOW → HIGH para obtener el número de vueltas que realiza la reductora, y así, como veremos más

adelante, obtener el valor de la velocidad.

Las variables que usamos dentro de una interrupción, como definimos en el apartado 3.1.7 deben ser del tipo

“volatile”.

void doencoderB()

cI++;

vueltasI=cI/8.0;

Figura 4-3. Función de Interrupción.

4.1 Interrupción de cambio de pin

A un nivel básico, una interrupción es una señal que interrumpe la actividad normal de nuestro

microprocesador y este salta a atenderla.

El procesador ATmega en el corazón de Arduino tiene dos tipos diferentes de interrupciones hardware

externas: "externa" y "Cambio de PIN".

En Arduino Uno sólo hay dos pines de interrupción externa, INT0 y INT1, en los pines 2 y 3. Estas

interrupciones pueden ajustarse para disparar en flancos ascendentes o descendentes de la señal.

Tabla 4-1. Pines de Interrupción.

Modelo

Arduino INT0 INT1

UNO Pin 2 Pin 3

Para la activación de las interrupciones externas debemos usar la función attachInterrupt().

attachInterrupt(<INT>,<nombre_función>,<forma de activación>)

Figura 4-4. Función de interrupción externa.

Dicha función se encarga por nosotros de asignar la función <nombre_función> a la interrupción <INT> y será

reconocida cuando se produzca <forma de activación> en el pin asociado a <INT>.

Los diferentes tipos de activación han sido definidos en el apartado anterior.

19


Por otro lado, cuando necesitamos que sea otro pin el que realice la lectura de la interrupción, usaremos las

interrupciones de cambio de PIN. Estas interrupciones, a diferencia de las interrupciones anteriores que

estaban asociadas a un pin, están asociadas a un grupo de pines. Son provocados por igual en flancos

ascendentes o descendentes de la señal, por lo que es el código de interrupción el encargado de ajustar los

pines que reciben las interrupciones.

Las bibliotecas están diseñadas para manejar el cambio pin del Arduino e interrumpen tan pronto como sea

posible.

#include <PinChangeInterrupt.h>

#include <PinChangeInterruptBoards.h>

#include <PinChangeInterruptPins.h>

#include <PinChangeInterruptSettings.h>

Figura 4-5. Bibliotecas Interrupciones cambio de pin.

Esta librería nos aporta una función para simplificar la gestión de la interrupción muy parecida a la utilizada en

las interrupciones externas, y que se denomina “attachPCINT”. Esta función permite definir el pin donde se

reconocerá la interrupción de cambio en el pin y la rutina de atención a la interrupción, así como el método de

reconocimiento de la interrupción.

Por ejemplo:

attachPCINT(digitalPinToPCINT(encoderA),doencoderA,CHANGE)

Figura 4-6. Ejemplo Interrupción cambio de pin.

5 OBTENCIÓN DE LA VELOCIDAD Y LA

ACELERACIÓN

5.1 Velocidad

En líneas generales existen dos estrategias o algoritmos para calcular la velocidad utilizando encoders

incrementales, a tiempo fijo o a espacio fijo.

Tiempo fijo: consiste en contar el número de pulsos (en este caso grados) que transcurren durante un

tiempo constante determinado. La velocidad se obtiene dividiendo el ángulo (en este caso los grados

contados) por el tiempo fijo transcurrido.

Espacio fijo: consiste en cronometrar el tiempo que pasa en incrementar el ángulo un grado. La

velocidad se obtiene de la misma forma, dividiendo el ángulo (en este caso un grado) por el tiempo

medido.

Nosotros usaremos la estrategia del tiempo fijo. Por esto utilizamos una interrupción temporal para medir la

velocidad, como se describió anteriormente en el apartado 3.1.

ir (flag==1)

velI=((vueltasI-I)*1000.0/(millis()-time1))*(2*PI/48);

I=vueltasI;

Figura 5-1. Obtención de la velocidad.

En este código las variables ‘velI’ y ‘vueltasI’ corresponden a la velocidad y posición de la rueda izquierda. El

cuerpo del código se encuentra dentro de un condicional “if”, de manera que sólo se ejecuta cuando se cumple

la condición. La condición establecida es que entre cálculo y cálculo hayan pasado 10 ms, que como

indicamos en el apartado 3.1.6. será cuando la bandera sea 1. Por tanto, ese es el tiempo fijo escogido en este

caso.

Además, multiplicamos por 1000 para pasar los milisegundos a segundos y por 2𝜋

48 para obtener la velocidad

en radianes una vez pasada la reductora.

Para reducir los ruidos a alta frecuencia se emplea un filtro de Butterworth.

El filtro de Butterworth es un filtro de tipo IIR (Infinite Impulse Response). La respuesta en frecuencia del

21


filtro es extremadamente plana (con mínimas ondulaciones) en la banda pasante, es decir, mantiene la

información de frecuencias bajas casi intacta, hasta que llega a la frecuencia de corte. Este filtro puede

implementarse de diferentes órdenes. Se decidió utilizar una frecuencia de corte normalizada alta. De esta

manera, aunque se eliminan únicamente frecuencias altas y por tanto permanece menos ruido, se consigue que

la señal se retrase un poco menos. La frecuencia normalizada que se decidió utilizar, tras realizar varios

experimentos es Wn=0.0784.

En este caso se ha utilizado únicamente un filtro Butterworth de orden 2 y frecuencia de corte normalizada

igual a 0.0784.

Para este orden, la función de transferencia discreta del filtro es del tipo:

𝐻(𝑧) =𝐵1 + 𝐵2𝑧−1 + 𝐵3𝑧−2

𝐴1 + 𝐴2𝑧−1 + 𝐴3𝑧−2=

𝑌(𝑧)

𝑋(𝑧)

Y por tanto su ecuación en diferencias puede escribirse como:

𝑦𝑘 =−𝐴2𝑦𝑘−1 − 𝐴3𝑦𝑘−2 + 𝐵1𝑥𝑘 + 𝐵2𝑥𝑘−1 + 𝐵3𝑥𝑘−2

𝐴1

Para calcular los coeficientes del filtro se ha utilizado la función de Matlab denominada “butter”,

introduciendo como argumentos el orden deseado y la frecuencia de corte normalizada.

[B,A]=butter(n,Wn)

Figura 5-2. Función de Matlab filtro Butterworth.

De esta forma, la función devuelve como salida dos vectores, el primero con los coeficientes correspondientes

al numerador de la función de transferencia y el segundo con los correspondientes al denominador.

Obtendremos los valores del filtro:

A1=1; A2=-1.6544; A3=0.7059;

B1=0.0129; B2=0.0257; B3=0.0129;

La velocidad filtrada será wfi:

wfi=1.6544*wfI_1-0.7059*wfI_2+0.0129*velI+0.0257*velI_1+0.0129*velI_2;

wfI_2=wfI_1;

wfI_1=wfi;

velI_2=velI_1;

velI_1=velI;

wfi0=wfi;

Figura 5-3. Velocidad filtrada.

5.2 Característica estática

Se trata la representación gráfica de la relación velocidad corregida/PWM. Se pueden observar los resultados

en sentido horario (positivo) y anti horario (negativo).

Obtención de la velocidad y la aceleración

22

Figura 5-4. Característica estática.

Podemos observar la existencia de una zona muerta y la poca linealidad que presenta el motor.

Además, hicimos la misma prueba con el otro motor y el resultado fue diferente.

5.3 Zona muerta y compensador

La zona muerta se trata de aquella donde la sensibilidad del instrumento es nula, lo que hace que no cambie su

indicación y señal de salida.

En nuestro caso se trata de la región donde la tensión aplicada no tiene efecto sobre la velocidad (no inicia el

giro). Una vez que la tensión aplicada excede los límites de la zona muerta el motor comenzará a girar.

Para el motor B, el usado en dicho proyecto, los valores de esta zona son:

Tabla 5-1. Zona muerta.

PWM Motor B

D. Horaria 38

D. Anti horaria 40

Finalmente, para compensar la zona muerta en el programa de control se suma el valor PWM correspondiente

a la señal procedente del controlador, teniendo en cuenta que los posibles problemas de saturación se corrigen

mediante un condicional, como veremos en el apartado 6.2.

5.4 Signo de la velocidad

Obtendremos el signo de la velocidad a través de la aceleración, ya que la velocidad que obtenemos del

encoder está en valor absoluto.

23


Para ello usaremos el siguiente código:

signoI=wfi+aceli*0.01;

if (signoI<0)

sI=AH;

else sI=H;

En el añadimos a la velocidad obtenida del encoder la aceleración por el tiempo (10ms). Este resultado nos

indica el signo que tiene la velocidad.

Si la velocidad es positiva el termino sI será igual a H, que indica que el sentido de giro es horario; si es

negativa sI será AH, indicando sentido anti horario.

5.5 Aceleración

Para calcular la aceleración el algoritmo que se sigue es análogo al que se decidió utilizar para la velocidad,

tras la argumentación en apartados anteriores. Esto es, se trata de un algoritmo a tiempo fijo.

En concreto, para calcular la aceleración en la práctica, se realiza dentro del mismo condicional usado para la

velocidad, de manera que la aceleración también se calcula cada 10 ms. La expresión que resume el cálculo de

la aceleración es:

𝑎𝑐𝑒𝑙𝑒𝑟𝑎𝑐𝑖ó𝑛 =𝑣𝑒𝑙𝑜𝑐𝑖𝑑𝑎𝑑 𝑓𝑖𝑙𝑡𝑟𝑎𝑑𝑎 𝑎𝑐𝑡𝑢𝑎𝑙 − 𝑣𝑒𝑙𝑜𝑐𝑖𝑑𝑎𝑑 𝑓𝑖𝑙𝑡𝑟𝑎𝑑𝑎 𝑎𝑛𝑡𝑒𝑟𝑖𝑜𝑟

𝑡𝑖𝑒𝑚𝑝𝑜 𝑡𝑟𝑎𝑛𝑠𝑐𝑢𝑟𝑟𝑖𝑑𝑜

En el código la aceleración es ‘aceli’:

aceli=(wfi-wfi0)/0.01;

Figura 5-5. Calculo de la aceleración.

6 CONTROL EN VELOCIDAD

ara el control en velocidad de nuestro motor, siguiendo los estudios de otros proyectos se decide utilizar

un controlador PI.

Figura 6-1. Diagrama de bloques PI.

Para obtener los valores del controlador PI calculamos la respuesta en escalón para PWM de 50 a 150 en

sentido anti horario (figura 6-2.) y PWM de 60 a 150 en sentido horario (figura 6-3.).

Figura 6-2. Sentido anti horario.

Figura 6-3. Sentido horario.

P

25


La función de transferencia tensión-velocidad de un motor de corriente continua es del tipo:

𝐺(𝑠) =𝑘

𝜏𝑠 + 1

donde k=ganancia estática=𝑠𝑎𝑙𝑖𝑑𝑎(∞)−𝑠𝑎𝑙𝑖𝑑𝑎(0)

𝑒𝑛𝑡𝑟𝑎𝑑𝑎(∞)−𝑒𝑛𝑡𝑟𝑎𝑑𝑎(0)=

𝑤(∞)−𝑤(0)

𝑃𝑊𝑀(∞)−𝑃𝑊𝑀(0)

τ=cte. de tiempo=t (63.2% w (∞))

Obtenemos los siguientes valores para el Motor B:

> sentido anti horario

K=3.069

τ=428ms 0.4s

> sentido horario

K=3.211

τ=258ms 0.3s

6.1 PI en velocidad

Para obtener el controlador PI tomamos como Kp inicial el valor de K del modelo y como Ti el valor de τ.

Sabemos que la ecuación del Pi es:

𝑢𝑘 = 𝑘𝑝 ∗ (𝑒𝑖 +1

𝑇𝑖∑ 𝑒𝑖

𝑘

𝑖=0

)

Donde:

𝑢𝑘=Kp*𝑒𝑖+Ki*It 𝑒𝑖=𝑟𝑘-𝑦𝑘 It= It+(0.01* 𝑒𝑖)

Y 𝑢𝑘 nunca debe superar el valor de 255.

Se pensó utilizar la tabla de ajuste de Ziegler-Nichols, esto se suele hacer para tomar los valores iniciales del

controlador que después se ajustan a mano.

Primero, tras obtener la respuesta en escalón para la velocidad (Figura 6-3.) se toman los siguientes datos de la

gráfica:

T1 (tiempo muerto). Este es el tiempo que tarda el sistema en comenzar a responder.

T2 (tiempo de subida). Este tiempo se calcula desde el punto en el que una recta tangente a la señal

de salida del sistema corta al valor inicial del sistema hasta el punto en el que la recta tangente llega al

valor final del sistema.

dX. Variación de la señal escalón.

dY. Variación de la respuesta del sistema.

Control en Velocidad

26

A partir de estos valores se puede calcular la constante del sistema Ko:

𝐾𝑜 =𝑑𝑋 ∗ 𝑇2

𝑑𝑌 ∗ 𝑇1

Con lo cual la tabla de valores para ajustar el controlador PID será la siguiente:

Tabla 6-1. Tabla Ziegler-Nichols.

Kp Ki Kd

P Ko

PI 0.9*Ko 0.27*Ko/T1

PID 1.2*Ko 0.60*Ko/T1 0.60*Ko*T1

Pero cuando se implementó en la práctica este controlador, no se conseguía llegar a cumplir las condiciones

del ensayo que se indican en el algoritmo de Ziegler-Nichols, por lo que el ajuste se hizo de forma

experimental, comenzando con los valores obtenidos en el apartado anterior.

El método experimental consiste básicamente en el ajuste iterativo de los parámetros del controlador a partir

de la observación de la respuesta temporal del sistema realimentado, y de la experiencia del equipo a las

tendencias de las variables controladas en función de los parámetros que se quieren ajustar.

Para el análisis experimental del ajuste del PI se deben seguir unas reglas heurísticas de ajuste. Los pasos a

seguir se explican a continuación.

-Paso 1: Acción Proporcional

Lo primero que se debe calcular de forma aproximada experimentalmente es el término proporcional, para esto

se debe poner el tiempo integral, Ti en su máximo valor ya que según la fórmula va dividiendo y se consigue

así anular este término.

En el caso de que hubiera tiempo derivativo, Td, se usaría su valor mínimo también para anular el término.

En este primer paso se empieza con ganancia del controlador baja y esta se va aumentando hasta obtener las

características de respuesta deseadas.

-Paso 2: Acción integral

En este paso se deberá reducir hasta anular el error en estado estacionario, aunque la oscilación sea excesiva.

Para poder ajustar mejor el controlador también se disminuye el valor de la ganancia, es decir Kp, ligeramente.

Esto se repetirá de forma iterativa hasta obtener las características de respuesta deseadas.

27


Tabla 6-2. Características que se ven afectadas al ajustar el controlador.

Característica Kp aumenta Ti disminuye Td aumenta

Estabilidad Se reduce Disminuye Aumenta

Velocidad Aumenta Aumenta Aumenta

Error estacionario No eliminado Eliminado No eliminado

Área del error Se reduce Disminuye Se reduce

Perturbación control Aumenta

bruscamente

Aumenta

gradualmente

Aumenta

bruscamente

Frecuencia lazo No afecta hasta

cierto punto

Disminuye Aumenta

El código usado para la obtención del valor que debe tomar la señal PWM, que nosotros llamaremos uI, para

alcanzar la velocidad dada por la referencia rI será:

eI=rI-wfi;

Iti=Iti+(0.01*eI);

if (abs(Iti)>255) // anti-windup

if (Iti>0)

Iti=255;

else Iti=-255;

uI=38+kpI*eI+kiI*Iti; // 38 es el compensador de la zona muerta, más error proporcional por ganancia más

error integral por ganancia

if (uI>255)

uI=255;

if(uI<0)

uI=0;

if (rI<0)

sI=AH;

rI=-rI;

Control en Velocidad

28

Serial.println(uI);

Figura 6-4. Control PI en velocidad.

Aquí es donde añadiremos el compensador de zona muerta, valor indicado en el apartado 5.3. que sumamos a

uI.

Finalmente, los valores del controlador serán:

KpI=9

KiI=6

El resultado de dicho control para una serie de referencias será:

Figura 6-5. Control PI en velocidad(rev/s).

Donde la señal negra es la referencia, la roja la señal PWM y la azul la velocidad controlada.

Las oscilaciones que se observan vienen de la reductora y se debe a que la fricción no es uniforme.

29


Si realizamos cambios de referencia de positiva a negativa el resultado es:

Figura 6-6. Cambio de referencia positiva a negativa(rev/s).

7 CONTROL EN ACELERACIÓN

ste tipo de control se lleva a cabo debido a que la idea inicial de proyecto era la construcción de un mini-

segway, el cual requería un control en aceleración para su manejo.

Para el control en aceleración decidimos también usar un PI.

Inicialmente tomamos el mismo valor de Kp que el PI en velocidad y modificamos el valor de Ki para que se

aproxime lo máximo posible a la referencia.

El código usado en este caso es:

eaI=raI-aceli;

Iati=Iati+(0.01*eaI);

if (abs(Iati)>255)

if (Iati>0)

Iati=255;

else Iati=-255;

aI=kpaI*eaI+kiaI*Iati;

if (aI>255)

aI=255;

Serial.println(aI);

Serial.println(raI);

vI=vI+(aI*((millis()-time3)*0.001)); // Actualización de la señal PWM con la aceleración

obtenida del controlador

if (vI>255)

E

31


vI=255;

if(vI<0)

vI=0;

Serial.println(vI);

Figura 7-1. Obtencion PI en aceleración.

Donde aI será el valor de la aceleración controlada, raI la referencia y vI el valor de la señal PWM que debe

tomar el motor para alcanzar dicha aceleración.

Los valores finales de las variables de control son:

kpaI=1.2

kiaI=0.0005

En esta grafica mostramos finalmente los resultados obtenidos cuando se produce una variación en la

referencia de aceleración de +20→0→-20.

Se representa en verde el valor que sigue la señal PWM(vI), en azul la velocidad (wfi [rev/s]), en negro la

referencia(raI[rev/𝑠2]) y por último en rojo la aceleración controlada(aI[rev/𝑠2]).

Figura 7-2. Control PI en aceleración.

Cabe mencionar el enorme ruido que presentan las señales.

Control en Aceleración

32

En esta última gráfica observamos como reacciona el motor cuando inicialmente le damos una referencia

positiva en aceleración y a continuación una negativa, provocando al cabo de un tiempo un cambio de signo en

la velocidad.

Se representa en verde el valor que sigue la señal PWM(vI), en azul la velocidad (wfi [rev/s]), en negro la

referencia(raI[rev/𝑠2]) y por último en rojo la aceleración controlada(aI[rev/𝑠2]).

Figura 7-3. Gestión del signo de la velocidad.

APÉNDICE DE CÓDIGO

A. CÓDIGO 1.1. Pruebas del motor

// sentido de giro del motor

#define CW 0 // dirección de las agujas del reloj

#define CCW 1 // dirección inversa a las agujas del reloj

// definición de cada motor

#define MOTOR_A 0 //Motor derecho

#define MOTOR_B 1 // Motor Izquierdo

//asignación de pines

const byte PWMA = 3; // control de velocidad del motor A (PWM) apagado=0/Vmax=255

const byte PWMB = 11; // control de velocidad del motor B (PWM) apagado=0/Vmax=255

const byte DIRA = 12; // señal digital que controla la dirección de rotación del motor A

const byte DIRB = 13; // señal digital que controla la dirección de rotación del motor B

void setup()

Serial.begin(9600);

setupArdumoto(); // hilo que inicializa todos los pines del motor como salidas

void loop()

// funcionamiento del motor A

driveArdumoto(MOTOR_A, CCW, 255); // motor A a Vmax en dirección inversa a las agujas del reloj

Apéndice de Código

34

delay(1000); // espera de 1seg

driveArdumoto(MOTOR_A, CW, 127); // motor A a velocidad media en sentido de las agujas del reloj

delay(1000); //espera de 1seg

stopArdumoto(MOTOR_A); // STOP motor A

// funcionamiento del motor B

driveArdumoto(MOTOR_B, CCW, 255); // motor B a Vmax en dirección inversa a las agujas del reloj


driveArdumoto(MOTOR_B, CW,127 ); // motor B a velocidad media en sentido de las agujas del reloj


stopArdumoto(MOTOR_B); // STOP motor B

// funcionamiento de ambos motores simultáneos

//primero a máxima velocidad en dirección de las agujas del reloj

driveArdumoto(MOTOR_A, CW, 255);

driveArdumoto(MOTOR_B, CW, 255);

delay(1000);

// después a velocidad media en sentido inverso

driveArdumoto(MOTOR_A, CCW, 127); // Motor A a media velocidad

driveArdumoto(MOTOR_B, CCW, 127); // Motor B a media velocidad

// función driveArdumoto: recibe motor (motor al que nos referimos A, B), dir (dirección agujas del reloj o

inversa) y spd (velocidad de giro)

void driveArdumoto(byte motor, byte dir, byte spd)

if (motor == MOTOR_A)

digitalWrite(DIRA, dir);

analogWrite(PWMA, spd);

else if (motor == MOTOR_B)

digitalWrite(DIRB, dir);

analogWrite(PWMB, spd);

35


// función stopArdumoto para el motor

void stopArdumoto(byte motor)

driveArdumoto(motor, 0, 0);

// hilo setupArdumoto inicializa los pins como salidas a nivel bajo

void setupArdumoto()

pinMode(PWMA, OUTPUT);

pinMode(PWMB, OUTPUT);

pinMode(DIRA, OUTPUT);

pinMode(DIRB, OUTPUT);

digitalWrite(PWMA, LOW);

digitalWrite(PWMB, LOW);

digitalWrite(DIRA, LOW);

digitalWrite(DIRB, LOW);

B. CÓDIGO 1.2. Control en velocidad





#define encoderB 2

#define encoderB1 8

long unsigned int time1;



volatile float vueltasI;

#define AH 1

#define H 0

#define MOTOR_B 1

const byte PWMB=11;


36

const byte DIRB=13;

volatile long unsigned int cI=0;

float I=0;

double velI=0;

double vel=0;

double aceli=0;

volatile int flag=0,f=0,g=0,h=0;

double wfI_2=0;

double wfI_1=0;

double wfi=0;

double velI_2=0;

double velI_1=0;

double wfi0=0;

int i=0,k=0,j;

float rI=0;

float eI=0;

float Iti=0;

float kpI=9;

float kiI=6;

float uI=0;

char sI=H;

int signoI=0;

void setup()


TCCR1A = 0; //inicializamos los registros de control

TCCR1B = 0;

TCNT1 = 0;

OCR1A = 2499; // valor CTC que se guarda en el registro OCR1A ¿10ms? 100/0.004-1=24999

// el 0.004 sale de multiplicar el preescalador 64 por el tiempo de reloj del arduino 16KHz

0.0000625*64=0.004

TCCR1B |= (1 << WGM12); // modo output compare

TCCR1B |= (1 << CS10); // preescalador de 64



interrupts();

37


Serial.begin(9600);

pinMode(encoderB,INPUT_PULLUP);

attachInterrupt (0,doencoderB,CHANGE);

time1=millis();

time2=millis();

time3=millis();

setupArdumoto();

void loop()

if (flag==1)

k=k+1;

driveArdumoto(MOTOR_B,sI,uI);


I=vueltasI;

Serial.println("velocidad izquierda");

Serial.println(velI);


Serial.println(wfi);

Serial.println(millis()-time2);

wfI_2=wfI_1;

wfI_1=wfi;

velI_2=velI_1;

velI_1=velI;


Serial.println(aceli);


if (signoI<0)

sI=AH;

else sI=H;


38

wfi0=wfi;

time1=millis();

delay(1);

eI=rI-wfi;

Iti=Iti+(0.01*eI);

if (abs(Iti)>255)

if (Iti>0)

Iti=255;

else Iti=-255;

uI=38+kpI*eI+kiI*Iti;

if (uI>255)

uI=255;

if(uI<0)

uI=0;

Serial.println(uI);

Serial.println(rI);


flag=1;

void doencoderB()

cI++;

vueltasI=cI/8.0;


39


if (motor == MOTOR_B)










C. CÓDIGO 1.3. Control en aceleración





#define encoderB 2




volatile float vueltasI;

#define AH 1

#define H 0

#define MOTOR_B 1

const byte PWMB=11;

const byte DIRB=13;


40

volatile long unsigned int cI=0;

float I=0;

double velI=0;

double aceli=0;

volatile int flag=0,f=0,g=0,h=0;

double wfI_2=0;

double wfI_1=0;

double wfi=0;

double velI_2=0;

double velI_1=0;

double wfi0=0;

double afI_2=1;

double afI_1=1;

double acelfi=1;

char sI=H;

char sD=H;

int signoI=0;

int signoD=0;

int i=0,k=0,j;

float raI=0;

float eaI=0;

float Iati=0;

float kpaI=1.2;

float kiaI=0.0005;

float aI=0;

float vI=55;

void setup()


TCCR1A = 0; //inicializamos los registros de control

TCCR1B = 0;

TCNT1 = 0;

OCR1A = 2499; // valor CTC que se guarda en el registro OCR1A ¿10ms? 100/0.004-1=24999

// el 0.004 sale de multiplicar el preescalador 64 por el tiempo de reloj del arduino 16KHz

0.0000625*64=0.004

TCCR1B |= (1 << WGM12); // modo output compare


41




interrupts();

Serial.begin(9600);

pinMode(encoderB,INPUT_PULLUP);

attachInterrupt (0,doencoderB,CHANGE);

time1=millis();

time2=millis();

time3=millis();

setupArdumoto();

void loop()

if (flag==1)

k=k+1;

driveArdumoto(MOTOR_B,sI,vI);


I=vueltasI;

Serial.print("velocidad izquierda");

Serial.println(velI);


Serial.println(wfi);

Serial.println(millis()-time2);

wfI_2=wfI_1;

wfI_1=wfi;

velI_2=velI_1;

velI_1=velI;


Serial.println(aceli);


if (signoI<0)


42

sI=AH;

else sI=H;

wfi0=wfi;

time1=millis();

delay(1);

eaI=raI-aceli;

Iati=Iati+(0.01*eaI);

if (abs(Iati)>255)

if (Iati>0)

Iati=255;

else Iati=-255;

aI=kpaI*eaI+kiaI*Iati;

if (aI>255)

aI=255;

Serial.println(aI);

Serial.println(raI);

vI=vI+(aI*((millis()-time3)*0.001));

if (vI>255)

vI=255;

if(vI<0)

vI=0;

Serial.println(vI);

flag=0;

time3=millis();

43



flag=1;

void doencoderB()

cI++;

vueltasI=cI/8.0;


if (motor == MOTOR_B)










Documents

Desarrollo de un Equipo de Control de un Motor de ...bibing.us.es/proyectos/abreproy/91081/fichero/memoria+MPG.pdf · 3 Montaje y Prueba del Funcionamiento de los Motores 11 ... 3Desarrollo