Escolar Documentos
Profissional Documentos
Cultura Documentos
Motores Brushless PDF
Motores Brushless PDF
Título
Autor/es
Director/es
Facultad
Departamento
Curso Académico
2015-2016
Diseño y desarrollo de un gimbal con motores brushless, trabajo fin de grado
de John Alexander Montoya Osorio, dirigido por Javier Rico Azagra y Montserrat Gil
Martínez (publicado por la Universidad de La Rioja), se difunde bajo una Licencia
Creative Commons Reconocimiento-NoComercial-SinObraDerivada 3.0 Unported.
Permisos que vayan más allá de lo cubierto por esta licencia pueden solicitarse a los
titulares del copyright.
© El autor
© Universidad de La Rioja, Servicio de Publicaciones,
publicaciones.unirioja.es
E-mail: publicaciones@unirioja.es
ESCUELA TÉCNICA SUPERIOR DE INGENIERÍA INDUSTRIAL
TITULACIÓN: Grado en
Ingeniería Electrónica Industrial y Automática
TÍTULO:
Diseño y desarrollo de un gimbal con motores brushless
INDICE GENERAL
ÍNDICE GENERAL
Contenido
MEMORIA ....................................................................................................................... 7
1. INTRODUCCIÓN ........................................................................................................9
8. CONCLUSIONES ..................................................................................................... 80
ANEXOS ......................................................................................................................... 81
Memoria
ÍNDICE DE LA MEMORIA
1. NTRODUCCIÓN ......................................................................................................... 4
8. CONCLUSIONES ..................................................................................................... 77
1. NTRODUCCIÓN
Las áreas científico técnicas en las que se enmarca este trabajo son,
principalmente, el Área de Ingeniería de Sistemas y Automática, y el de Tecnología
Electrónica
1.1. ANTECEDENTES
AÑO 2015
ENERO FEBRERO MARZO ABRIL MAYO JUNIO
T1
T2
T3
T4
T5
T6
T7
T8
T9
T10
1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4
AÑO 2015
JULIO AGOSTO SETIEMBRE OCTUBRE NOVIEMBRE DICIEMBRE
T1
T2
T3
T4
T5
T6
T7
T8
T9
T10
1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4
AÑO 2016
ENERO FEBRERO MARZO ABRIL MAYO JUNIO
T1
T2
T3
T4
T5
T6
T7
T8
T9
T10
1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4
2.1. FUNDAMENTOS
Un gimbal de dos ejes está compuesto por una estructura mecánica que
Por último, las acciones de control generadas por los dos lazos de control
son traducidas en señales de excitación para los motores BLDC. Es decir, cada una
de las señales es procesada dando lugar a una lógica de conmutación de las fases
del motor.
2.2. MATERIALES
3. SISTEMA DE COMUNICACIONES
Las líneas SDA y SCL para un dispositivo conectado al bus deben ser del tipo
drenaje abierto, es decir, un estado similar al de colector abierto, pero asociadas a
un transistor de efecto de campo (o FET). Se deben polarizar en estado alto
(conectando a la alimentación por medio de resistores "pull-up") lo que define una
estructura de bus que permite conectar en paralelo múltiples entradas y salidas.
Cuando el bus esta libre las dos líneas permanecen en estado alto para
iniciar una comunicación el master debe generar una condición de START. El
primer byte que se transmite luego de la condición de inicio contiene siete bits que
componen la dirección del dispositivo que se desea seleccionar, y un octavo bit que
corresponde a la operación que se quiere realizar con él (lectura o escritura).
Definición de términos:
• Bus libre (Bus Free): Estado en el que ambas líneas (SDA y SCL)
están inactivas, presentando un estado lógico alto. Es el único
momento en que un dispositivo maestro puede comenzar a hacer
uso del bus.
𝒄𝒐𝒔(𝝍) 𝒔𝒊𝒏(𝝍) 𝟎
𝑹𝒗𝟏
𝑰 𝝍 = −𝒔𝒊𝒏(𝝍) 𝒄𝒐𝒔(𝝍) 𝟎
𝟎 𝟎 𝟏
El pitch representa una rotación sobre el eje Y del S.R V1 un ángulo θ como
se muestra en la figura 4-3. Es importante darse cuenta de que ángulo de pitch no
es una rotación sobre el eje y del S.R inercial.
𝒄𝒐𝒔(𝜽) 𝟎 −𝒔𝒊𝒏(𝜽)
𝑹𝒗𝟐
𝒗𝟏 𝜽 = 𝟎 𝟏 𝟎
𝒔𝒊𝒏(𝜽) 𝟎 𝒄𝒐𝒔(𝜽)
𝟏 𝟎 𝟎
𝑹𝑩
𝒗𝟐 𝝓 = 𝟎 𝒄𝒐𝒔(𝝓) 𝒔𝒊𝒏(𝝓)
𝟎 −𝒔𝒊𝒏(𝝓) 𝒄𝒐𝒔(𝝓)
La matriz de rotación que nos traslada de S.R inercial hasta el S.R del objeto
viene dada por :
𝑩 𝒗𝟐 𝒗𝟏
𝑹𝑩
𝑰 (𝝓, 𝜽, 𝝍) = 𝑹𝒗𝟐 𝝓 𝑹𝒗𝟏 𝜽 𝑹𝑰 𝝍
4.1.2. CUATERNIOS
qbi a b c d T
𝑎 = 𝒄𝒐𝒔 𝟎. 𝟓𝜽
𝑏 = 𝑽𝒙 𝒔𝒊𝒏
(𝟎. 𝟓𝜽)
𝑐 = 𝑽𝒚 𝒔𝒊𝒏 𝟎. 𝟓𝜽
𝑑 = 𝑽𝒛 𝒔𝒊𝒏 𝟎. 𝟓𝜽
El cuaternio anterior puede ser usado para rotar cualesquier vector de tres
elementos desde el sistema de referencia inercial hasta el sistema anclado al
sensor o S.R B usando la siguiente operación:
0 −1
VB = qbi qbi
VI
Esto significa que un vector puedo ser rotado tratándolo como un cuaterno
con parte real igual a cero y multiplicándolo por el cuaternio de orientación y su
inversa.
T
q1 = a1 b1 c1 d1
T
q 2 = a2 b2 c2 d2
Para rotar un vector desde el S.R del objeto al S.R inercial se requiere
efectuar una multiplicación de dos cuaternios tales como los anteriormente
definidos. Alternativamente, el cuaternio de orientación puede ser usado para
construir una matriz de rotación de3x3 y llevar a cabo la rotación en una simple
operación. La matriz de rotación desde el sistema inercial hasta el sistema del
objeto se define como:
Por lo tanto la rotación del sistema inercial al sistema del cuerpo puede ser
llevada a cabo usando la matriz de multiplicación:
VB = Rbi qbi VI
ϕ atan2(2 ab + cd , 1 − 2(b2 + c 2 ))
θ = arcsin(2(ac − db))
ψ atan2(2 ad + bc , 1 − 2(c 2 + d2 ))
4.2.2. ACELERÓMETROS
Los acelerómetros son dispositivos que miden la aceleración, que es la tasa
de cambio de la velocidad de un objeto. Esto se mide en metros por segundo al
cuadrado (m/s²) o en las fuerzas G (g). La sola fuerza de la gravedad corresponde
aproximadamente con 9,8 m/s², pero este valor fluctúa ligeramente con la altitud.
Los acelerómetros son útiles para detectar las vibraciones en los sistemas o para
aplicaciones que requieran el conocimiento de la orientación.
𝒂𝒙
𝜽𝒚 = 𝐚𝐭𝐚𝐧
𝒂𝒛
𝟏
𝐆 𝐬 =
𝟎. 𝟏𝐬 + 𝟏
Bode Diagram
0
Magnitude (dB)
-10
-20
-30
-40
0
Phase (deg)
-45
-90
-1 0 1 2 3
10 10 10 10 10
Frequency (rad/s)
Ilustración 4-13. Diagrama de bode de un filtro paso-bajo
60
40
Angulo ( grados )
X: 1.24
Y: 27.24
20
-20
-40
-60
0 1 2 3 4 5 6
Tiempo ( seg )
Ilustración 4-14. Resultados del primero experimento
En las figuras 4-16 y 4-17 aparece el análisis espectral de cada una de las
señales. En la primera se observa como la señal que no ha sido filtrada presenta
magnitudes significativas para las frecuencias altas, sin embargo en la segunda
figura se ve claramente como después de aplicar el filtro, las componentes de alta
frecuencia han sido eliminadas.
X: 1.8
Y: -0.6256
-0.64
-0.66
Vel. Angular ( º/s )
-0.68
-0.7
-0.72
-0.74
0 1 2 3 4 5 6 7
Tiempo ( seg )
FFT Magnitude
500
450 X: 0.1429
Y: 477.2
400
350
300
250
200
150
100
50
0
0 5 10 15 20 25 30 35 40 45 50
Freq ( Hz )
0.1s
G s =
0.1s + 1
Bode Diagram
0
-20
-30
-40
-50
90
Phase (deg)
60
30
0
-1 0 1 2 3
10 10 10 10 10
Frequency (rad/s)
Ilustración 4-19. Diagrama de bode de un filtro paso-alto
-2
-8
-10
-12
-14
-16
-18
0 1 2 3 4 5 6
Tiempo ( seg )
En la figura 4-22 aparecen los resultados del mismo experimento pero esta
vez el sensor gira entorno a uno de los ejes. Como se puede ver en la gráfica azul no
existe acumulación error; a partir de los 4 segundos el sensor se deja en reposo y
se observa que el ángulo no varía, sin embargo en la grafía verde se ve claramente
como el ángulo sigue variando a lo largo del tiempo debido a que en la señal existe
una componente continua que no ha sido eliminada.
Exp3. Angulo estimado con giroscopio
40
0
Angulo ( grados )
-20
-40
-60
-80
0 1 2 3 4 5 6
Tiempo ( seg )
Ilustración 4-21. Estimación del ángulo girado mediante acelerómetro
8000
6000
4000
2000
0
0 5 10 15 20 25 30 35 40 45 50
Freq ( Hz )
Ilustración 4-22. FFT de las medidas de un acelerómetro
Exp3. Analisis en frecuencia
4000
3000
2500
2000
1500
1000
500
0
0 5 10 15 20 25 30 35 40 45 50
Freq ( Hz )
Ilustración 4-23. FFT de las medidas de un acelerómetro una vez filtradas
𝑯𝒂 𝒔 𝑮 𝒔 + 𝑯𝒈 𝒔 𝟏 − 𝑮 𝒔 =𝟏
𝜶
𝑮(𝒔) =
𝒔+𝜶
𝒔
𝟏 − 𝑮(𝒔) =
𝒔+𝜶
Este filtro nos permite hacer que las componentes de alta frecuencia de la
medición estimada dominadas por el aporte de las mediciones provenientes del
giroscopio.
𝜽 𝒔 = 𝜽(𝒔)
80
Angulo Giro
60
Angulo Acc
Angulo Filtro Complementario
40
Angulo ( grados )
20
-20
-40
-60
0 1 2 3 4 5 6
Tiempo ( seg )
En el rotor (la parte del motor que gira) se incrustan imanes permanentes
de neodimio. El numero de imanes adheridos al rotor determina el numero de
conmutaciones que han de ocurrir para el motor gire una vuelta completa. Los
imanes se van colocando uno seguido del otro y alternando su polaridad tal y como
se muestra en la figura 5-6.
Ilustración 5-7. Campo magnético generado por las bobinas del estator
Tabla 1
Paso Bobina A Bobina B Bobina C
1 Vdc No conectada 0V
2 No conectada Vdc 0V
3 0V Vdc No conectada
4 OV No conectada Vdc
5 No conectada 0V Vdc
6 Vdc 0V No conectada
Por otra parte, el ángulo girado tras cada conmutación depende del número
de par de polos que hay en el rotor. Como ejemplo se dirá que para los motores
usados en este proyecto es necesario completar 7 ciclos eléctricos para dar una
vuelta entera puesto que el rotor tiene 7 pares de polos, con esta disposición el
ángulo girado por cada conmutación resultaría:
𝑝𝑎𝑠𝑜𝑠
7 𝑐𝑖𝑐𝑙𝑜𝑠 ∗ 6 = 42 𝑝𝑎𝑠𝑜𝑠
𝑐𝑖𝑐𝑙𝑜
360 𝑔𝑟𝑎𝑑𝑜𝑠
𝜃𝑔𝑖𝑟𝑎𝑑𝑜 = = 8.57 𝑔𝑟𝑎𝑑𝑜𝑠 𝑝𝑜𝑟 𝑝𝑎𝑠𝑜
42 𝑝𝑎𝑠𝑜𝑠
Tanto el rizado del par como la poca resolución de giro hacen que esta
técnica resulte ineficiente para usarla en un gimbal real en donde es necesario
corregir ángulos de giro muy pequeños de decimas o incluso centésimas de grado.
Debido a estos inconvenientes se presenta necesario el uso de un método que
minimice estos efectos como bien puede ser el SPWM el cual se explica en el
siguiente apartado.
Lo que se obtiene con este patrón de corriente es producir una salida de par
de mayor valor y que presenta un menor rango de oscilación en comparación al
par obtenido cuando se conmuta con la técnica de 6 pasos.
Para modular una señal sinusoidal mediante PWM existen varias formas,
una de las más sencillas resulta de la comparación entre dos formas de onda, la
señal de referencia o señal a modular (señal modulada) se compara con una señal
de acarreo triangular de alta frecuencia (señal de acarreo). El resultado de esta
comparación se usa para generar una señal cuadrada la cual es una representación
modulada de la onda original. En el caso de la técnica SPWM, lo que se persigue
reproducir una señal sinusoidal mediante la modulación de una señal cuadrada.
𝑝𝑎𝑠𝑜𝑠
7 𝑐𝑖𝑐𝑙𝑜𝑠 ∗ 9 = 63 𝑝𝑎𝑠𝑜𝑠
𝑐𝑖𝑐𝑙𝑜
360 𝑔𝑟𝑎𝑑𝑜𝑠
𝜃𝑔𝑖𝑟𝑎𝑑𝑜 = = 5.7 𝑔𝑟𝑎𝑑𝑜𝑠 𝑝𝑜𝑟 𝑝𝑎𝑠𝑜
63 𝑝𝑎𝑠𝑜𝑠
𝑝𝑎𝑠𝑜𝑠
7 𝑐𝑖𝑐𝑙𝑜𝑠 ∗ 540 = 3780 𝑝𝑎𝑠𝑜𝑠
𝑐𝑖𝑐𝑙𝑜
360 𝑔𝑟𝑎𝑑𝑜𝑠
𝜃𝑔𝑖𝑟𝑎𝑑𝑜 = = 0.095 𝑔𝑟𝑎𝑑𝑜𝑠 𝑝𝑜𝑟 𝑝𝑎𝑠𝑜
3780 𝑝𝑎𝑠𝑜𝑠
Ilustración 5-20.Ilustración
Par y velocidad
5-21. con
Par conmutación a 6conmutación
y velocidad con pasos 6 step
6. SISTEMA DE CONTROL
El eje Z del sensor se monta de manera que este alineado con la dirección de
la fuerza de gravedad tal y como aparece representado en la figura 6-2. Los
controladores de ambos motores deben en todo momento corregir cualesquier
alteración en los ángulos de roll y pitch con el fin de conservar la orientación de la
base.
𝐾𝑝
𝐺=
𝑠 ∗ (1 + 𝑡𝑝1 ∗ 𝑠)
50
X: 1.62
Grados
40 Y: 50.4
30
20
10
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5
2.5
2
Accion de control
1.5
0.5
-0.5
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5
Tiempo
𝟑𝟎𝟐. 𝟏𝟏
𝑮=
𝒔 ∗ (𝟏 + 𝟎. 𝟎𝟗𝟗𝟏𝟕𝟒 ∗ 𝒔)
50
40
30
20
10
-10
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time
30
25
20
15
10
Step Response
1
0.8
0.7
0.6
Amplitude 0.5
0.4
0.3
0.2
0.1
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9
Time (seconds)
30
Grados
20
10
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
0.8
0.6
Acc. control
0.4
0.2
-0.2
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Tiempo
𝟐𝟗𝟕. 𝟏𝟐
𝑮=
𝒔 ∗ (𝟏 + 𝟎. 𝟎𝟑𝟕𝟏𝟔𝟗 ∗ 𝒔)
35
30
25
20
15
10
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time
Ilustración 6-10. Comparación ente el modelo real el modelo estimado
50
40
30
20
10
-10
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time
Ilustración 6-11. Comparación entre plantas
𝟐𝟗𝟕. 𝟏𝟐
𝑮𝟏 = 𝟖𝟖. 𝟒𝟎𝟒𝟓 % 𝒅𝒆 𝒂𝒑𝒓𝒐𝒙.
𝒔 ∗ (𝟏 + 𝟎. 𝟎𝟑𝟕𝟏𝟔𝟗 ∗ 𝒔)
302.11
𝐺2 = 81.8566 % 𝑑𝑒 𝑎𝑝𝑟𝑜𝑥
𝑠 ∗ (1 + 0.099174 ∗ 𝑠)
Root Locus Editor for Open Loop 1(OL1) Open-Loop Bode Editor for Open Loop 1(OL1)
100 150
100
80
50
60
Magnitude (dB)
0
40
-50
20
-135
-40
Phase (deg)
-180
-60
-225
-80
(𝟏 + 𝟐𝟎 ∗ 𝒔)
𝑪 = 𝟎. 𝟎𝟎𝟏𝟑𝟐𝟑𝟑 Ecuacion 5.16
𝒔 ∗ (𝟏 + 𝟎. 𝟎𝟏𝟐 ∗ 𝒔)
30
25
20
15
10
0
0 50 100 150 200 250 300
Step Response
1.2
0.6
Amplitude
0.4
0.2
-0.2
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9
Time (seconds)
Ilustración 6-14. Respuesta esperada tras aplicar el filtro
Integra también una memoria FIFO on-chip de 1024 bytes que almacena las
medidas y ayuda al ahorro de energía ya que el dispositivo puede ser puesto en
modo de bajo consumo y seguir tomando muestras desde los sensores.
Este motor entrega el par necesario para mover masas que no superen los
300 – 400 gramos, según su hoja de características. Además del par él, motor es de
un tamaño reducido y resulto ser muy apropiado y eficiente.
7.5. PCB
El diseño del circuito ha sido realizado mediante KiCad por ser este un
software gratuito y bastante completo para el diseño de PCBs.
En la figuras 7-7 y 7-8 aparece el una captura del layout del PCB una vez se
ha concluido el trazado de las pistas y vías.
8. CONCLUSIONES
Vías de continuación:
ANEXOS
ÍNDICE DE ANEXOS
2. CODIGO ................................................................................................................. 11
En este capitulo se describen las librerías que ha sido necesario generar para llevar
a cabo las diversas tareas asociadas al control de posición de los motores. Entre
tales tareas se contempla por poner un ejemplo, el uso del protocolo de
comunicación I2C para la transferencia de datos entre el microprocesador y la
unidad de sensores. Este requerimiento exige la elaboración o uso de alguna
librería que implemente el protocolo y que proporcione las funciones apropiadas
para tal fin.
Parámetro Descripción
uint32_t period Indica el periodo de tiempo que transcurre entre
cada interrupción y se corresponde con el numero
de ciclos de reloj necesarios para incrementar el
valor de un contador desde cero hasta el valor de
period.
void(*task)(void) Es un puntero a la función que se va a ejecutar
cada vez que se produzca la interrupción.
• void Timer0A_Handler(void)
• void Timer1A_Handler(void)
• void Timer2A_Handler(void)
void TimerXA_Handler(void)
Parámetro Descripción
ninguno
Esta librería incluye tres funciones, una función sirve para inicializar los
módulos I2C del mircroprocesador TM4C123GP6Z y las otras dos se usan para
enviar y recibir datos mediante el protocolo I2C.
• void I2C0_Init()
void I2C0_Init(void)
Esta función envía un byte de datos por el bus I2C a la dirección indicada
en addr
Parámetro Descripción
unsigned char addr addr contiene la dirección del dispositivo en el
que se quiere acceder
Parámetro Descripción
unsigned char addr addr contiene la dirección del dispositivo en el
que se quiere acceder
La librería PWM incluye tres funciones de las cuales una se usa para
configurar el modulo PMW1 del microprocesador TM4C123GP6Z y las dos
funciones restante modulan una señal trifásica manipulando los registros del
modulo de manera síncrona.
Esta función inicializa el modulo PWM1 para que este genere 6 salidas de
PWM a una frecuencia determinada a través de los pines: PF0,
PF2,PF3,PD0,PD1 y PA6.
Parámetro Descripción
uint16_t period Con el valor de indicado en period se calcula la
frecuncia de conmutación de la señal PWM
Esta función genera una señal trifásica en los pines PF0, PF2 y PF3, si X =
1
Esta función genera una señal trifásica en los pines PDO,PD1 y PA6, si X
=0
Parámetro Descripción
int8_t increment Incremente puede tomar el valor de cero o uno
y determina la dirección de avance de las
señales moduladas para hacer mover el motor
Esta librería incluye una sola función. En ella se configura la frecuencia del
reloj del sistema y puede ser modificada si se modifica el valor del parámetro
SISDIV2 que aparece en el fichero: PLL.h
• void PLL_Init(void)
void PLL_Init(void)
Parámetro Descripción
ninguno
• void EnableInterrupts(void):
void EnableInterrupts(void)
• void WaitForInterrupt(void):
Estas dos funciones manipulan los registros del TIMER0 y TIMER1 y son
usadas para variar la frecuencia de las señales moduladas. En base al
valor de freq/freq2, se calcula el numero de ciclos de reloj necesarios
para generar una sinusoide a la frecuencia deseada.
Parámetro Descripción
freq, freq2 El valor asignado a freq/freq2 corresponde con
la velocidad de giro del motor en
revoluciones/segundo.
• void Calibrate_Gyros()
void Calibrate_Gyros()
Esta función sirve para calcular el drift que presentan los giroscopios con
el fin de extraer esta componente de las medidas. Solo se ejecuta una vez
al arrancar la aplicación.
Parámetro Descripción
ninguno
• void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y,
float a_z)
void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y, float
a_z)
Parametro Descripción
w_x, w_y, w_z Velocidad angular de cada uno de los
giroscopios
a_x, a_y, a_z Aceleración de cada uno de los acelerómetros
• void OutputSineWave(void)
• void OutputSineWave2(void)
void OutputSineWave(void)
void OutputSineWave2(void)
Estas dos funciones se ejecutan cada que se produce una interrupción que bien
puede ser del TIMER0 O TIMER1. Al mismo tiempo dentro de ellas se ejecutan los
funciones que modulan las señales PWM con las que se hacen girar los motores
Parámetro Descripción
ninguno
• void PeriodTask(void)
void PeriodTask(void)
Parámetro Descripción
ninguno
• int main(void)
int main(void)
Parámetro Descripción
ninguno
2. CODIGO
// Fichero: PWM.h
// Alexander Montoya
// 01-07-2016
Fichero PWM.c:
// Fichero: PWM.c
// Alexander Montoya
// 01-07-2016
/*
*********************************************************
*********************************************************
** En esta librería se definen las 3 funciones que **
** son necesarias para la generación de 6 señales **
** PWM usando el modulo PWM del mircroprocesador **
** TM4C123G6PZ. Una de las funciones inicializa y **
** configuran el modulo y las otras dos se usan para **
** cambiar el ancho de pulso de las señales PWM **
** **
*********************************************************
*********************************************************
*/
//
// Ficheros a incluir
//
#include <stdint.h>
#include "PWM.h"
#include <math.h>
//
// Definición de punteros a los registros del modulo
//
uint16_t Wave2[1000];
GPIO_PORTF_LOCK_R |= 0x4C4F434B;
GPIO_PORTF_CR_R |= 0x01;
GPIO_PORTF_PCTL_R = (GPIO_PORTF_PCTL_R&0xFFFF00F0)+0x00005505; //
Configura PF0/2/3 para soportar señales de PWM
GPIO_PORTD_PCTL_R = (GPIO_PORTD_PCTL_R&0xFFFFFF00)+0x00000055; //
Configura PD0/1 para soportar señales de PWM
GPIO_PORTA_PCTL_R = (GPIO_PORTA_PCTL_R&0xF0FFFFFF)+0x05000000; //
Configura PA6 para soportar señales de PWM
PWM1_0_GENB_R = 0xB00;
PWM1_0_GENA_R = 0x0B0;
PWM1_1_GENA_R = 0x0B0;
PWM1_2_GENA_R = 0x0B0;
PWM1_3_GENA_R = 0x0B0;
PWM1_3_GENB_R = 0xB00;
uint8_t ft=1;
PWM1_CTL_R |= 0x0C;
}
uint8_t ft2=1;
PWM1_CTL_R |= 0x03;
}
Fichero Timers.h:
// Fichero: Timers.h
// Alexander Montoya
// 01-07-2016
#endif // __TIMER0AINTS_H__
Fichero Timers.c:
// Fichero: Timers.h
// Alexander Montoya
// 01-07-2016
/*
*********************************************************
*********************************************************
** En esta librería se definen las definen las **
** funciones usadas para generar interrupciones **
** periódicas a una frecuencia particular haciendo **
** uso de los módulos Timer del microporcesador **
** TM4C123G6PZ **
*********************************************************
*********************************************************
*/
#include <stdint.h>
#include "tm4c123gh6pm.h"
Fichero I2C.h:
// Fichero : I2C.h
// Alexander Montoya Osorio
// 01/06/2016
#ifndef I2C_H
#define I2C_H
void I2C0_Init(void);
unsigned long writeMD(unsigned char addr, unsigned char addrReg, unsigned char
data);
unsigned long readMD(unsigned char addr,unsigned char regAddr, unsigned char
numbytes, unsigned char *databuff);
#endif
Fichero I2C.c:
// Fichero : I2C.c
// Alexander Montoya Osorio
// 01/06/2016
/*
*********************************************************
*********************************************************
** En esta librería se definen las funciones **
** necesarias para inicializar el modulo **
** I2C0 del microprocesador TM4C123G6PZ y usarlo **
** para llevar a cabo transferencias de datos **
** entre el micro y la IMU **
** **
*********************************************************
*********************************************************
*/
//
// Ficheros a incluir
//
#include "I2C.h"
#include <stdint.h>
#include "tm4c123gh6pm.h"
//
// Definición de procedimientos
//
#define SLA_W(address) (address << 1)
#define SLA_R(address) ((address << 1) + 0x01)
//
// Definición de etiquetas
//
#define I2C_MCS_ACK 0x00000008 // Data Acknowledge Enable
#define I2C_MCS_DATACK 0x00000008 // Acknowledge Data
#define I2C_MCS_ADRACK 0x00000004 // Acknowledge Address
#define I2C_MCS_STOP 0x00000004 // Generate STOP
#define I2C_MCS_START 0x00000002 // Generate START
#define I2C_MCS_ERROR 0x00000002 // Error
#define I2C_MCS_RUN 0x00000001 // I2C Master Enable
#define I2C_MCS_BUSY 0x00000001 // I2C Busy
#define I2C_MCR_MFE 0x00000010 // I2C Master Function Enable
void I2C0_Init()
{
SYSCTL_RCGCI2C_R |= 0x0001; // 1) Activa el modulo I2CO
SYSCTL_RCGCGPIO_R |= 0x0002; // 2) Activa el puerto B
while((SYSCTL_PRGPIO_R&0x0002) == 0){};// ready? Bucle de espera mientras se
activa el puerto
unsigned long writeMD(unsigned char addr, unsigned char addrReg, unsigned char
data) // write
{
// Añade bit de escritura
I2C0_MSA_R = SLA_W(addr);
if((I2C0_MCS_R&(I2C_MCS_ADRACK|I2C_MCS_ERROR)) != 0)
{
I2C0_MCS_R = (0 | I2C_MCS_STOP | I2C_MCS_RUN);
return 0;
}
Fichero gimbal.c:
// Fichero: gimbal.c
// Alexander Montoya Osorio
// 01/06/2016
/*
*********************************************************
*********************************************************
** Este es el programa principal que se ejecuta
** al arrancar el gimbal. Se debe garantizar la inclusión
** de todos los ficheros necesario. El reloj del sistema
** se configura a 50 Mhz y la frecuencia de muestreo
** se establece en 200 Hz
*********************************************************
*********************************************************
*/
//
// Ficheros a incluir
//
#include "tm4c123gh6pm.h"
#include <stdint.h>
#include "PLL.h"
#include "Timer0A.h"
#include <stdio.h>
#include <stdlib.h> // C99 variable types
#include <math.h>
#include "PWM.h"
#include "I2C.h"
//
// Constantes del sistema
//
#define deltat 1.0f/500.0f // sampling period in seconds (shown as 1 ms)
#define gyroMeasError 3.14159265358979f * (5.0f / 180.0f) // gyroscope
measurement error in rad/s (shown as 5 deg/s)
//
// Funciones del sistema
//
void DisableInterrupts(void); // Disable interrupts
void EnableInterrupts(void); // Enable interrupts
long StartCritical (void); // previous I bit, disable interrupts
void EndCritical(long sr); // restore I bit to previous value
void WaitForInterrupt(void); // low power mode
//
// Acción de control para cada uno de los motores
//
void func(double freq);
void func2(double freq2);
//
// Calibración de los giros
//
void Calibrate_Gyros(void);
//
// Variables globales del sistema
//
float a_x1, a_y1, a_z1; // medidas de los acelerómetros en m/s^2
int16_t ia_x, ia_y, ia_z; // medidas del acelerómetro
float SEq_1 = 1.0f, SEq_2 = 0.0f, SEq_3 = 0.0f, SEq_4 = 0.0f; // estimated orientation
quaternion elements with initial conditions
float eR,eP;
float errorR;
uint8_t datArray[14];
float yaw,roll,pitch;
float AngleIn = 0 ;
float AngleRef = 0;
float AngleOut = 0 ;
int count = 1;
int8_t increment = 0;
int8_t increment2 = 0;
float ref=0;
float
GYRO_XOUT_OFFSET_1000SUM,GYRO_YOUT_OFFSET_1000SUM,GYRO_ZOU
T_OFFSET_1000SUM;
float GYRO_XOUT_OFFSET,GYRO_YOUT_OFFSET,GYRO_ZOUT_OFFSET;
//
// Avance sinuoide trifasica para el Roll
//
void OutputSineWave(void)
{
SPWM_Modulation1(increment);
func(errorR);
//
// Avance sinuoide trifasica para el Pitch
//
void OutputSineWave2(void)
{
SPWM_Modulation2((increment2);
func2(errorP);
}
float u,u_m1,u_m2,u_m3;
float e,e_m1,e_m2,e_m3;
float u2,u2_m1,u2_m2,u2_m3;
float eP,e2_m1,e2_m2,e2_m3;
//
// Lectura de datos
//
unsigned long leido = readMD(0x68,0x3B,0x14,datArray);
//
// Eliminar bias de los giros
//
w_x1 = iw_x - GYRO_XOUT_OFFSET;
w_y1 = iw_y - GYRO_YOUT_OFFSET;
w_z1 = iw_z - GYRO_ZOUT_OFFSET;
//
// Cambio de unidades a rad/s
//
w_x1 = -w_x1*PI/180;
w_y1 = -w_y1*PI/180;
w_z1= w_z1*PI/180;
a_x1 = ia_x;
a_y1 = ia_y;
a_z1 = -ia_z;
//
// Aplicación filtro Madwick
//
filterUpdate(w_x1/SCALE_GYRO,w_y1/SCALE_GYRO,w_z1/SCALE_GYRO,a_x
1/SCALE_ACC,a_y1/SCALE_ACC,a_z1/SCALE_ACC);
//
// Calculo de angulos Roll, Pitch y Yaw
//
roll = atan2(2*SEq_1*SEq_2+2*SEq_3*SEq_4,1-2*SEq_2*SEq_2-
2*SEq_3*SEq_3)*180/PI;
pitch = asin(2*SEq_1*SEq_3 - 2*SEq_4*SEq_2)*180/PI;
yaw = atan2(2*SEq_1*SEq_4 - 2*SEq_3*SEq_2 ,1-2*SEq_3*SEq_3-
2*SEq_4*SEq_4)*180/PI;
//
// Calculo del error
//
eR = ref - roll;
//
// Calculo accion de control
//
u = 0.004564*eR + 0.00001141*e_m1 - 0.004563*e_m2 + 1.655*u_m1 -
0.6552*u_m2 ; // pid Roll
u2 = 0.004564*eP + 0.00001141*e2_m1 - 0.004563*e2_m2 + 1.655*u2_m1 -
0.6552*u2_m2 ; // pid Pitch
u_m3 = u_m2;
u_m2 = u_m1;
u_m1 = u;
e_m3 = e_m2;
e_m2 = e_m1;
e_m1 = eR;
u2_m3 = u2_m2;
u2_m2 = u2_m1;
u2_m1 = u2;
e2_m3 = e2_m2;
e2_m2 = e2_m1;
e2_m1 = eP;
errorP = u2;
errorR = u;
}
//
// Configura reloj del sistema
//
PLL_Init():
//
// Configuración IMU
//
writeMD(0x68,0x1A,0x06); // Activamos filtro interno de la IMU
writeMD(0x68,0x1B,0x18); // Gyro scale to 2000 º/s
writeMD(0x68,0x1C,0x00); // Acel scale to 2g
writeMD(0x68,0x6B,0x02); // Sleep disable
//
// Calibración de sensores
//
Calibrate_Gyros();
//
// Inicialización modulo PWM1
//
PWM1_Init(1300);
//
// Inicialización de los Timer
//
Timer0A_Init(&OutputSineWave2, 190476);
Timer1A_Init(&OutputSineWave, 190476);
Timer2A_Init(&PeriodTask, FS);
EnableInterrupts();
while(1){
WaitForInterrupt();
}
return 0;
}
if (freq == 0) increment = 0;
else{
uint32_t P;
double vel = (1/(freq*3780))*50000000;
P = fabs(vel);
if (freq2 == 0) increment2 = 0;
else{
uint32_t P;
double vel = (1/(freq2*3780))*50000000;
GYRO_XOUT_OFFSET_1000SUM += iw_x;
GYRO_YOUT_OFFSET_1000SUM += iw_y;
GYRO_ZOUT_OFFSET_1000SUM += iw_z;
GYRO_XOUT_OFFSET = GYRO_XOUT_OFFSET_1000SUM/1000;
GYRO_YOUT_OFFSET = GYRO_YOUT_OFFSET_1000SUM/1000;
GYRO_ZOUT_OFFSET = GYRO_ZOUT_OFFSET_1000SUM/1000;
}
Fichero PLL.h:
// Fichero: PLL.h
// Alexander Montoya Osorio
// 01/06/2016
#define SYSDIV2 7
// bus frequency is 400MHz/(SYSDIV2+1) = 400MHz/(7+1) = 50 MHz
/*
SYSDIV2 Divisor Clock (MHz)
0 1 reserved
1 2 reserved
2 3 reserved
3 4 reserved
4 5 80.000
5 6 66.667
6 7 reserved
7 8 50.000
8 9 44.444
9 10 40.000
10 11 36.364
11 12 33.333
12 13 30.769
13 14 28.571
14 15 26.667
15 16 25.000
16 17 23.529
17 18 22.222
18 19 21.053
19 20 20.000
20 21 19.048
21 22 18.182
22 23 17.391
23 24 16.667
24 25 16.000
25 26 15.385
26 27 14.815
27 28 14.286
28 29 13.793
29 30 13.333
30 31 12.903
31 32 12.500
Fichero PLL.c :
// Fichero: PLL.c
// Alexander Montoya Osorio
// 01/06/2016
/*
*********************************************************
*********************************************************
** En esta librería se define una función para **
** inicializar el reloj del sistema a una frecuencia **
** determinada por el usuario haciendo uso del PLL **
** **
*********************************************************
*********************************************************
*/
#include "PLL.h"
Planos
ÍNDICE DE PLANOS
CONN_01X03 CONN_01X03
P2
P3
1
2
3
1
2
3
L6234_1 L6234_2
1 20 1 20
OUT1 OUT2 OUT1 OUT2
A 2 19 2 19 A
PWM4 IN1 IN2 PWM6 PWM1 IN1 IN2 PWM2
3 18 3 18
ENABLE EN1 EN2 ENABLE ENABLE EN1 EN2 ENABLE
4 17 4 17
Vs SENSE1 Vs SENSE1
5 16 5 16
GND GND GND GND
6 15 6 15
GND GND GND GND
GND 7 14 GND GND 7 14 GND
Vs SENSE2 Vs SENSE2
C7
C4
GND
GND
C
C
8 13 8 13
ENABLE EN3 VBOOT ENABLE EN3 VBOOT
9 12 9 12
PWM5 IN3 VCP PWM3 IN3 VCP
10 11 10 11
OUT3 VREF OUT3 VREF
C10 C12
C9
C6
D3
D1
VCC
D
CP CP
D C D
C
D4 GND D2
GND
C3 C8
B B
CP C
GND
P1
+VBUS
1
2
3
4
3
2
1
VCC VCC
+VBUS
7805
CONN_01X02
ENABLE
P4
1 1
VI VO
3
GND GND 2
GND
J3-J1 J2-J4
2 1 2 1 C1 C2
ENABLE PWM1 GND U1
2
4 3 4 3 PWM3
C 6 5 6 5 C C C
PWM4 GND
CONN_02X10
CONN_02X10
PWM5
8 7 PWM2
8 7 PWR_FLAG PWR_FLAG
10 9 10 9
12 11 12 11
14 13 14 13 GND
16 15 16 15
18 17 18 17 GND VCC
PWM6
20 19 20 19
UNIVERSIDAD DE LA RIOJA
Sheet: /
D File: PCB.sch D
A A
B B
C C
D D
E E
UNIVERSIDAD DE LA RIOJA
Sheet:
File: noname3.kicad_pcb
Title: Capa inferior del PCB
Size: A4 Date: 2016-07-06 Rev: 1.0
KiCad E.D.A. pcbnew 4.0.2-stable Id: 1/1
F F
1 2 3 4
1 2 3 4
A A
B B
C C
D D
E E
UNIVERSIDAD DE LA RIOJA
Sheet:
File: noname3.kicad_pcb
Title: Capa superior del PCB
Size: A4 Date: 2016-07-06 Rev: 1.0
KiCad E.D.A. pcbnew 4.0.2-stable Id: 1/1
F F
1 2 3 4
Diseño y desarrollo de un gimbal
con motores brushless
Pliego de condiciones
1. DISPOSICIONES GENERALES
1.1. OBJETO
2. DEFINICIÓN Y ALCANCE
3.1. PCB
Tabla 1
3.2. TARJETA
• Microcontrador TM4C123GH6PMZ
• Motion control PWM
• Conectores USB micro-A y micro-B para su programación y
depurado
• Un led RGB de uso general
• Dos botones de uso general
• On-board ICDI
• Switch de selección de alimentación
• Boton de Reset
• Dos conectores de 20 pines
Parámetro Valor
Tensión de alimentación De 4,7 a 5,25 voltios de continua desde uno
de las siguientes fuentes:
• Debugger (ICDI) USB Micro-B conectado a
un PC
• USB Micro-B puesto host
Dimensiones de la tarjeta:
3.3. REGULADOR
Parametro Dimensión
en mm
A 4.40
E 10
e 2.4
L30 28.9
L 13
3.1. L6432
Parámetro Dimension
en mm
D 24.8
E 8.8
e 2.54
F 7.1
L 3.3
3.2. IMU
Características adicionales:
Parámetros Rango
VDD. Alimentación 2.375 V a 3.465V
Frecuencia de com. I2C 100Khz a 400Khz
Temperaturas max de operacion -40 a +85 °C
Direcciones I2C 1101000
3.3. MOTORES
Parámetro Valor
KV 100
Peso 55 g
Dist. Entre tornillos 16*19 mm
Métrica M3
Estructura 12N14P
Alimentación 3S Li-Poly
Dimensiones estator 28*6 mm
• PID
• Frecuencia de muestreo
• Parámetro β
La estructura del gimbal no debería estar nunca de manera que los ejes de
los motores estén alineados. Tal disposiciónhará que la estimación de los ángulos
de Roll y Pitch sea incorrecta por que se produce una singularidad matemática
dentro del modelo usado para la estimación. Por este motivo se debe prestar
atención en para evitar que se alineen los ejes.
Los motores del Gimbal han sido diseñados para funcionar con baterías tipo
LiPo de 3 celdas, por lo que el uso de cualesquier otro tipo de batería no garantiza
el correcto funcionamiento del sistema ya que una tensión de alimentación
indebida puede producir calentamiento de los motores y una tensión muy baja
puede suponer una pérdida del par de giro.
Las conexiones entre todos los elementos deben estar garantizadas para
que el Gimbal funcione bien. Cabe resaltar que un fallo en las comunicaciones va a
suponer que el sistema se comporte de manera indeterminada.
7. CONDICIONES ECONÓMICAS
7.2. LIQUIDACIÓN
7.3. DISPOSICIÓN
ÍNDICE
• Módulos electrónicos
• PCB
• Cableado
• Elementos mecánicos
• Recursos humanos
Móduloselectrónicos
Referencia Unidad Unidades Nombre del elemento
EK-TM4C123GLX unidades 1 LauchPadEvaluation Kit
L6234 unidades 2 Driver para motor trifásico
MPU unidades 1 Unidad Inercial de medida
Batería unidades 1 Fuente de tensión
Elementos mecanicos
Referencia Unidad Unidades Nombre del elemento
EMAX-GB2806 unidades 2 Motor bruhsless
Estructura metálica unidades 1 Estructura Gimbal
Tornillo M3 unidades 8 Tornillo motor-estructura
1.3. PCB:
La referencia a cada elemento corresponde con la notación usada en los planos del
PCB.
PCB
Referencia Unidad Unidades Nombre del elemento
PCB unidades 1 Circuito impreso diseñado
C1 unidades 1 Condensador cerámico 0,33µF/50V
C6 Y C9 unidades 2 Condensadorcerámico 10nF/25V
C4 y C7 unidades 2 Condensador cerámico 0,22µF/50V
C2 y C8 unidades 2 Condensadorcerámico 0,1µF/25V
C10 y C12 unidades 2 Condensador electrolítico 1µF
C3 unidades 1 Condensador electrolítico 100µF
D1, D2, D3 y D4 unidades 4 Diodo 1N4148
L6234_1 y L6234_2 unidades 2 Conector DIP20
J3-J1 y J2-J4 unidades 2 Conector hembra 20 pines. 2x10
P5 unidades 1 Conector hembra 3 pines
7805L unidades 1 Regulador de tensión
P4 unidades 1 Conector batería
P1 unidades 1 Conector IMU
P2 y P3 unidades 2 Conector motor
1.4. CABLEADO:
Cableado
Referencia Unidad Unidades Nombre del elemento
Cable IMU 1 Cable de alimentación sensor y bus I2C
Cable motor 1 1 Cable motor
Cable motor 2 1 Cable motor
Recursos humanos
Referencia Unidad Unidades Nombre del elemento
Investigaciónde técnicas de
RH1 Horas 150
conmutación para motores BLDC
RH2 Horas 150 Investigación en tratamiento de datos
RH3 Horas 300 Programación
RH4 Horas 30 Diseño PCB
RH5 Horas 20 Diseño estructura mecánica
RH6 Horas 90 Redacción del documento
Presupuesto
1. PRESUPUESTO .........................................................................................................3
1. PRESUPUESTO
Para el cálculo se presupone para una sola unidad Gimbal, por lo que los
precios que aparecen para cada unidad corresponden con el precio normal de
mercado para un usuario común.
• Módulos electrónicos
• PCB
• Cableado
• Elementos mecánicos
• Recursos humanos
2. CUADRO DE PRECIOS
Moduloselectronicos
Referencia Unidad Nombre del elemento Precio/Ud
EK-TM4C123GLX unidades LauchPadEvaluation Kit 13 €
L6234 unidades Driver para motor trifásico 5,84 €
MPU unidades Unidad Inercial de medida 3,91 €
Batería unidades Fuente de tensión 10,46 €
PCB
Referencia Unidad Nombre del elemento Precio/Ud
PCB unidades Circuito impreso diseñado 10 €
C1 unidades Condensador cerámico 0,33µF/50V 0,181 €
C6 Y C9 unidades Condensador cerámico 10nF/25V 0,187 €
C4 y C7 unidades Condensador cerámico 0,22µF/50V 0,124 €
C2 y C8 unidades Condensador cerámico 0,1µF/25V 0,154 €
C10 y C12 unidades Condensador electrolítico 1µF 0,136 €
C3 unidades Condensador electrolítico 100µF 0,152 €
D1, D2, D3 y D4 unidades Diodo 1N4148 0,300 €
L6234_1 y L6234_2 unidades Conector DIP20 0,500 €
J3-J1 y J2-J4 unidades Conector hembra 20 pines. 2x10 2,500 €
P5 unidades Conector hembra 3 pines 0,500 €
7805L unidades Regulador de tensión 1,950 €
P4 unidades Conector batería 0,320 €
P1 unidades Conector IMU 0,320 €
P2 y P3 unidades Conector motor 0,320 €
Cableado
Referencia Unidad Nombre del elemento Precio/Ud
Cable de alimentación sensor y bus
Cable IMU I2C 0,50 €
Cable motor 1 Cable motor 0,50 €
Cable motor 2 Cable motor 0,50 €
Recursos humanos
Referencia Unidad Nombre del elemento Precio/Ud
Investigación técnicas de
RH1 Horas 30 €
conmutación para motores BLDC
Investigación en tratamiento de
RH2 Horas datos 30 €
RH3 Horas Programación 30 €
RH4 Horas Diseño PCB 30 €
RH5 Horas Diseño estructura 30 €
RH6 Horas Redacción documento 30 €
Presupuesto móduloselectrónicos:
Móduloselectrónicos
Referencia Unidad Unidades Nombre del elemento Precio/Ud Subtotal
EK-TM4C123GLX Ud 1 LauchPadEvaluation Kit 13 € 13,00 €
L6234 Ud 2 Driver para motor trifasico 5,84 € 11,68 €
MPU Ud 1 Unidad Inercial de medida 3,91 € 3,91 €
Batería Ud 1 Fuente de tensión 10,46 € 10,46 €
Subtotal 39,05 €
Presupuesto cableado:
Cableado
Referencia Unidad Unidades Nombre del elemento Precio/Ud Sub Total
Cable de alimentación sensor y bus
Cable IMU Ud 1 I2C 0,50 € 0,50 €
Cable motor 1 Ud 1 Cable motor 0,50 € 0,50 €
Cable motor 2 Ud 1 Cable motor 0,50 € 0,50 €
Subtotal 1,50 €
Presupuesto PCB:
PCB
Referencia Unidad Unidades Nombre del elemento Precio/Ud Subtotal
PCB Ud 1 Circuito impreso diseñado 10 € 10,000 €
C1 Ud 1 Condensador cerámico 0,33µF/50V 0,181 € 0,181 €
C6 Y C9 Ud 2 Condensadorcerámico 10nF/25V 0,187 € 0,374 €
C4 y C7 Ud 2 Condensador cerámico 0,22µF/50V 0,124 € 0,248 €
C2 y C8 Ud 2 Condensadorcerámico 0,1µF/25V 0,154 € 0,308 €
C10 y C12 Ud 2 Condensador electrolítico 1µF 0,136 € 0,272 €
C3 Ud 1 Condensador electrolítico 100µF 0,152 € 0,152 €
D1, D2, D3 y D4 Ud 4 Diodo 1N4148 0,300 € 1,200 €
L6234_1 y L6234_2 Ud 2 Conector DIP20 0,500 € 1,000 €
J3-J1 y J2-J4 Ud 2 Conector hembra 20 pines. 2x10 2,500 € 5,000 €
P5 Ud 1 Conector hembra 3 pines 0,500 € 0,500 €
7805L Ud 1 Regulador de tensión 1,950 € 1,950 €
Elementos mecanicos
Referencia Unidad Unidades Nombre del elemento Precio/Ud Sub Total
EMAX-GB2806 Ud 2 Motor bruhsless 15 € 30,00 €
Estructura metálica Ud 1 Estructura Gimbal 3€ 3,00 €
Tornillo M3 Ud 8 Tornillo motor-estructura 0,01 € 0,04 €
Subtotal 33,04 €
4. PRESUPUESTO GLOBAL
Suma______________________________________ 30.459,31 €