Professional Documents
Culture Documents
Ttulo
Autor/es
Director/es
Facultad
Departamento
Curso Acadmico
2015-2016
Diseo 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
Martnez (publicado por la Universidad de La Rioja), se difunde bajo una Licencia
Creative Commons Reconocimiento-NoComercial-SinObraDerivada 3.0 Unported.
Permisos que vayan ms 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 TCNICA SUPERIOR DE INGENIERA INDUSTRIAL
TITULACIN: Grado en
Ingeniera Electrnica Industrial y Automtica
TTULO:
Diseo y desarrollo de un gimbal con motores brushless
INDICE GENERAL
NDICE GENERAL
Contenido
MEMORIA ....................................................................................................................... 7
1. INTRODUCCIN ........................................................................................................9
8. CONCLUSIONES ..................................................................................................... 80
ANEXOS ......................................................................................................................... 81
Memoria
NDICE DE LA MEMORIA
1. NTRODUCCIN ......................................................................................................... 4
8. CONCLUSIONES ..................................................................................................... 77
1. NTRODUCCIN
Las reas cientfico tcnicas en las que se enmarca este trabajo son,
principalmente, el rea de Ingeniera de Sistemas y Automtica, y el de Tecnologa
Electrnica
1.1. ANTECEDENTES
AO 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
AO 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
AO 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 mecnica que
Por ltimo, las acciones de control generadas por los dos lazos de control
son traducidas en seales de excitacin para los motores BLDC. Es decir, cada una
de las seales es procesada dando lugar a una lgica de conmutacin de las fases
del motor.
2.2. MATERIALES
3. SISTEMA DE COMUNICACIONES
Las lneas 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 alimentacin por medio de resistores "pull-up") lo que define una
estructura de bus que permite conectar en paralelo mltiples entradas y salidas.
Cuando el bus esta libre las dos lneas permanecen en estado alto para
iniciar una comunicacin el master debe generar una condicin de START. El
primer byte que se transmite luego de la condicin de inicio contiene siete bits que
componen la direccin del dispositivo que se desea seleccionar, y un octavo bit que
corresponde a la operacin que se quiere realizar con l (lectura o escritura).
Definicin de trminos:
Bus libre (Bus Free): Estado en el que ambas lneas (SDA y SCL)
estn inactivas, presentando un estado lgico alto. Es el nico
momento en que un dispositivo maestro puede comenzar a hacer
uso del bus.
() ()
= () ()
El pitch representa una rotacin 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 rotacin sobre el eje y del S.R inercial.
() ()
=
() ()
= () ()
() ()
La matriz de rotacin 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 operacin:
0 1
VB = qbi qbi
VI
Esto significa que un vector puedo ser rotado tratndolo como un cuaterno
con parte real igual a cero y multiplicndolo por el cuaternio de orientacin y su
inversa.
T
q1 = a1 b1 c1 d1
T
q 2 = a2 b2 c2 d2
a1 a 2 b1 b2 c1 c2 d1 d2
a1 b 2 +b1 a2 +c1 d2 d1 c2
q1 q 2 =
a1 c2 b1 d2 +c1 a2 +d1 b2
a1 d2 +b1 c2 c1 b2 +d1 a2
Para rotar un vector desde el S.R del objeto al S.R inercial se requiere
efectuar una multiplicacin de dos cuaternios tales como los anteriormente
definidos. Alternativamente, el cuaternio de orientacin puede ser usado para
construir una matriz de rotacin de3x3 y llevar a cabo la rotacin en una simple
operacin. La matriz de rotacin desde el sistema inercial hasta el sistema del
objeto se define como:
2 + 2 2 2 2 2 2 + 2
= 2 + 2 2 + 2 2
2
2 2
2 2 2 + 2 2 2 2 + 2
Por lo tanto la rotacin del sistema inercial al sistema del cuerpo puede ser
llevada a cabo usando la matriz de multiplicacin:
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. ACELERMETROS
Los acelermetros son dispositivos que miden la aceleracin, 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 flucta ligeramente con la altitud.
Los acelermetros son tiles para detectar las vibraciones en los sistemas o para
aplicaciones que requieran el conocimiento de la orientacin.
=
. +
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)
Ilustracin 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 )
Ilustracin 4-14. Resultados del primero experimento
En las figuras 4-16 y 4-17 aparece el anlisis espectral de cada una de las
seales. En la primera se observa como la seal que no ha sido filtrada presenta
magnitudes significativas para las frecuencias altas, sin embargo en la segunda
figura se ve claramente como despus 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)
Ilustracin 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 grfica azul no
existe acumulacin error; a partir de los 4 segundos el sensor se deja en reposo y
se observa que el ngulo no vara, sin embargo en la grafa verde se ve claramente
como el ngulo sigue variando a lo largo del tiempo debido a que en la seal 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 )
Ilustracin 4-21. Estimacin del ngulo girado mediante acelermetro
8000
6000
4000
2000
0
0 5 10 15 20 25 30 35 40 45 50
Freq ( Hz )
Ilustracin 4-22. FFT de las medidas de un acelermetro
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 )
Ilustracin 4-23. FFT de las medidas de un acelermetro una vez filtradas
+ =
() =
+
() =
+
Este filtro nos permite hacer que las componentes de alta frecuencia de la
medicin 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.
Ilustracin 5-7. Campo magntico 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 conmutacin depende del nmero
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 elctricos para dar una
vuelta entera puesto que el rotor tiene 7 pares de polos, con esta disposicin el
ngulo girado por cada conmutacin resultara:
7 6 = 42
360
= = 8.57
42
Tanto el rizado del par como la poca resolucin de giro hacen que esta
tcnica resulte ineficiente para usarla en un gimbal real en donde es necesario
corregir ngulos de giro muy pequeos de decimas o incluso centsimas de grado.
Debido a estos inconvenientes se presenta necesario el uso de un mtodo que
minimice estos efectos como bien puede ser el SPWM el cual se explica en el
siguiente apartado.
Lo que se obtiene con este patrn de corriente es producir una salida de par
de mayor valor y que presenta un menor rango de oscilacin en comparacin al
par obtenido cuando se conmuta con la tcnica de 6 pasos.
Para modular una seal sinusoidal mediante PWM existen varias formas,
una de las ms sencillas resulta de la comparacin entre dos formas de onda, la
seal de referencia o seal a modular (seal modulada) se compara con una seal
de acarreo triangular de alta frecuencia (seal de acarreo). El resultado de esta
comparacin se usa para generar una seal cuadrada la cual es una representacin
modulada de la onda original. En el caso de la tcnica SPWM, lo que se persigue
reproducir una seal sinusoidal mediante la modulacin de una seal cuadrada.
7 9 = 63
360
= = 5.7
63
7 540 = 3780
360
= = 0.095
3780
Ilustracin 5-20.Ilustracin
Par y velocidad
5-21. con
Par conmutacin a 6conmutacin
y velocidad con pasos 6 step
6. SISTEMA DE CONTROL
El eje Z del sensor se monta de manera que este alineado con la direccin 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
alteracin en los ngulos de roll y pitch con el fin de conservar la orientacin 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
Ilustracin 6-10. Comparacin 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
Ilustracin 6-11. Comparacin 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)
Ilustracin 6-14. Respuesta esperada tras aplicar el filtro
Integra tambin una memoria FIFO on-chip de 1024 bytes que almacena las
medidas y ayuda al ahorro de energa 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, segn su hoja de caractersticas. Adems del par l, motor es de
un tamao reducido y resulto ser muy apropiado y eficiente.
7.5. PCB
El diseo del circuito ha sido realizado mediante KiCad por ser este un
software gratuito y bastante completo para el diseo 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 vas.
8. CONCLUSIONES
Vas de continuacin:
ANEXOS
NDICE DE ANEXOS
2. CODIGO ................................................................................................................. 11
En este capitulo se describen las libreras que ha sido necesario generar para llevar
a cabo las diversas tareas asociadas al control de posicin de los motores. Entre
tales tareas se contempla por poner un ejemplo, el uso del protocolo de
comunicacin I2C para la transferencia de datos entre el microprocesador y la
unidad de sensores. Este requerimiento exige la elaboracin o uso de alguna
librera que implemente el protocolo y que proporcione las funciones apropiadas
para tal fin.
Parmetro Descripcin
uint32_t period Indica el periodo de tiempo que transcurre entre
cada interrupcin 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 funcin que se va a ejecutar
cada vez que se produzca la interrupcin.
void Timer0A_Handler(void)
void Timer1A_Handler(void)
void Timer2A_Handler(void)
void TimerXA_Handler(void)
Parmetro Descripcin
ninguno
Esta librera incluye tres funciones, una funcin sirve para inicializar los
mdulos 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 funcin enva un byte de datos por el bus I2C a la direccin indicada
en addr
Parmetro Descripcin
unsigned char addr addr contiene la direccin del dispositivo en el
que se quiere acceder
Parmetro Descripcin
unsigned char addr addr contiene la direccin del dispositivo en el
que se quiere acceder
La librera 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 seal trifsica manipulando los registros del
modulo de manera sncrona.
Esta funcin inicializa el modulo PWM1 para que este genere 6 salidas de
PWM a una frecuencia determinada a travs de los pines: PF0,
PF2,PF3,PD0,PD1 y PA6.
Parmetro Descripcin
uint16_t period Con el valor de indicado en period se calcula la
frecuncia de conmutacin de la seal PWM
Esta funcin genera una seal trifsica en los pines PF0, PF2 y PF3, si X =
1
Esta funcin genera una seal trifsica en los pines PDO,PD1 y PA6, si X
=0
Parmetro Descripcin
int8_t increment Incremente puede tomar el valor de cero o uno
y determina la direccin de avance de las
seales moduladas para hacer mover el motor
Esta librera incluye una sola funcin. En ella se configura la frecuencia del
reloj del sistema y puede ser modificada si se modifica el valor del parmetro
SISDIV2 que aparece en el fichero: PLL.h
void PLL_Init(void)
void PLL_Init(void)
Parmetro Descripcin
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 seales 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.
Parmetro Descripcin
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 funcin 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 aplicacin.
Parmetro Descripcin
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 Descripcin
w_x, w_y, w_z Velocidad angular de cada uno de los
giroscopios
a_x, a_y, a_z Aceleracin de cada uno de los acelermetros
void OutputSineWave(void)
void OutputSineWave2(void)
void OutputSineWave(void)
void OutputSineWave2(void)
Estas dos funciones se ejecutan cada que se produce una interrupcin que bien
puede ser del TIMER0 O TIMER1. Al mismo tiempo dentro de ellas se ejecutan los
funciones que modulan las seales PWM con las que se hacen girar los motores
Parmetro Descripcin
ninguno
void PeriodTask(void)
void PeriodTask(void)
Parmetro Descripcin
ninguno
int main(void)
int main(void)
Parmetro Descripcin
ninguno
2. CODIGO
// Fichero: PWM.h
// Alexander Montoya
// 01-07-2016
Fichero PWM.c:
// Fichero: PWM.c
// Alexander Montoya
// 01-07-2016
/*
*********************************************************
*********************************************************
** En esta librera se definen las 3 funciones que **
** son necesarias para la generacin de 6 seales **
** 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 seales PWM **
** **
*********************************************************
*********************************************************
*/
//
// Ficheros a incluir
//
#include <stdint.h>
#include "PWM.h"
#include <math.h>
//
// Definicin 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 seales de PWM
GPIO_PORTD_PCTL_R = (GPIO_PORTD_PCTL_R&0xFFFFFF00)+0x00000055; //
Configura PD0/1 para soportar seales de PWM
GPIO_PORTA_PCTL_R = (GPIO_PORTA_PCTL_R&0xF0FFFFFF)+0x05000000; //
Configura PA6 para soportar seales 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 librera se definen las definen las **
** funciones usadas para generar interrupciones **
** peridicas a una frecuencia particular haciendo **
** uso de los mdulos 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 librera 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"
//
// Definicin de procedimientos
//
#define SLA_W(address) (address << 1)
#define SLA_R(address) ((address << 1) + 0x01)
//
// Definicin 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
{
// Aade 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 inclusin
** 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
//
// Accin de control para cada uno de los motores
//
void func(double freq);
void func2(double freq2);
//
// Calibracin de los giros
//
void Calibrate_Gyros(void);
//
// Variables globales del sistema
//
float a_x1, a_y1, a_z1; // medidas de los acelermetros en m/s^2
int16_t ia_x, ia_y, ia_z; // medidas del acelermetro
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;
//
// Aplicacin 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():
//
// Configuracin 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
//
// Calibracin de sensores
//
Calibrate_Gyros();
//
// Inicializacin modulo PWM1
//
PWM1_Init(1300);
//
// Inicializacin 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 librera se define una funcin 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
Diseo y desarrollo de un gimbal
con motores brushless
Pliego de condiciones
1. DISPOSICIONES GENERALES
1.1. OBJETO
2. DEFINICIN Y ALCANCE
3.1. PCB
Tabla 1
3.2. TARJETA
Microcontrador TM4C123GH6PMZ
Motion control PWM
Conectores USB micro-A y micro-B para su programacin y
depurado
Un led RGB de uso general
Dos botones de uso general
On-board ICDI
Switch de seleccin de alimentacin
Boton de Reset
Dos conectores de 20 pines
Parmetro Valor
Tensin de alimentacin 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 Dimensin
en mm
A 4.40
E 10
e 2.4
L30 28.9
L 13
3.1. L6432
Parmetro Dimension
en mm
D 24.8
E 8.8
e 2.54
F 7.1
L 3.3
3.2. IMU
Caractersticas adicionales:
Parmetros Rango
VDD. Alimentacin 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
Parmetro Valor
KV 100
Peso 55 g
Dist. Entre tornillos 16*19 mm
Mtrica M3
Estructura 12N14P
Alimentacin 3S Li-Poly
Dimensiones estator 28*6 mm
PID
Frecuencia de muestreo
Parmetro
= 1300
= 20
= 1300 20 = 26
1
. = = 38.461
0.000026
La estructura del gimbal no debera estar nunca de manera que los ejes de
los motores estn alineados. Tal disposicinhar que la estimacin de los ngulos
de Roll y Pitch sea incorrecta por que se produce una singularidad matemtica
dentro del modelo usado para la estimacin. Por este motivo se debe prestar
atencin en para evitar que se alineen los ejes.
Los motores del Gimbal han sido diseados para funcionar con bateras tipo
LiPo de 3 celdas, por lo que el uso de cualesquier otro tipo de batera no garantiza
el correcto funcionamiento del sistema ya que una tensin de alimentacin
indebida puede producir calentamiento de los motores y una tensin muy baja
puede suponer una prdida 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 ECONMICAS
7.2. LIQUIDACIN
7.3. DISPOSICIN
NDICE
Mdulos electrnicos
PCB
Cableado
Elementos mecnicos
Recursos humanos
Mduloselectrnicos
Referencia Unidad Unidades Nombre del elemento
EK-TM4C123GLX unidades 1 LauchPadEvaluation Kit
L6234 unidades 2 Driver para motor trifsico
MPU unidades 1 Unidad Inercial de medida
Batera unidades 1 Fuente de tensin
Elementos mecanicos
Referencia Unidad Unidades Nombre del elemento
EMAX-GB2806 unidades 2 Motor bruhsless
Estructura metlica unidades 1 Estructura Gimbal
Tornillo M3 unidades 8 Tornillo motor-estructura
1.3. PCB:
La referencia a cada elemento corresponde con la notacin usada en los planos del
PCB.
PCB
Referencia Unidad Unidades Nombre del elemento
PCB unidades 1 Circuito impreso diseado
C1 unidades 1 Condensador cermico 0,33F/50V
C6 Y C9 unidades 2 Condensadorcermico 10nF/25V
C4 y C7 unidades 2 Condensador cermico 0,22F/50V
C2 y C8 unidades 2 Condensadorcermico 0,1F/25V
C10 y C12 unidades 2 Condensador electroltico 1F
C3 unidades 1 Condensador electroltico 100F
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 tensin
P4 unidades 1 Conector batera
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 alimentacin sensor y bus I2C
Cable motor 1 1 Cable motor
Cable motor 2 1 Cable motor
Recursos humanos
Referencia Unidad Unidades Nombre del elemento
Investigacinde tcnicas de
RH1 Horas 150
conmutacin para motores BLDC
RH2 Horas 150 Investigacin en tratamiento de datos
RH3 Horas 300 Programacin
RH4 Horas 30 Diseo PCB
RH5 Horas 20 Diseo estructura mecnica
RH6 Horas 90 Redaccin del documento
Presupuesto
1. PRESUPUESTO .........................................................................................................3
1. PRESUPUESTO
Para el clculo 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 comn.
Mdulos electrnicos
PCB
Cableado
Elementos mecnicos
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 trifsico 5,84
MPU unidades Unidad Inercial de medida 3,91
Batera unidades Fuente de tensin 10,46
PCB
Referencia Unidad Nombre del elemento Precio/Ud
PCB unidades Circuito impreso diseado 10
C1 unidades Condensador cermico 0,33F/50V 0,181
C6 Y C9 unidades Condensador cermico 10nF/25V 0,187
C4 y C7 unidades Condensador cermico 0,22F/50V 0,124
C2 y C8 unidades Condensador cermico 0,1F/25V 0,154
C10 y C12 unidades Condensador electroltico 1F 0,136
C3 unidades Condensador electroltico 100F 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 tensin 1,950
P4 unidades Conector batera 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 alimentacin 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
Investigacin tcnicas de
RH1 Horas 30
conmutacin para motores BLDC
Investigacin en tratamiento de
RH2 Horas datos 30
RH3 Horas Programacin 30
RH4 Horas Diseo PCB 30
RH5 Horas Diseo estructura 30
RH6 Horas Redaccin documento 30
Presupuesto mduloselectrnicos:
Mduloselectrnicos
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
Batera Ud 1 Fuente de tensin 10,46 10,46
Subtotal 39,05
Presupuesto cableado:
Cableado
Referencia Unidad Unidades Nombre del elemento Precio/Ud Sub Total
Cable de alimentacin 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 diseado 10 10,000
C1 Ud 1 Condensador cermico 0,33F/50V 0,181 0,181
C6 Y C9 Ud 2 Condensadorcermico 10nF/25V 0,187 0,374
C4 y C7 Ud 2 Condensador cermico 0,22F/50V 0,124 0,248
C2 y C8 Ud 2 Condensadorcermico 0,1F/25V 0,154 0,308
C10 y C12 Ud 2 Condensador electroltico 1F 0,136 0,272
C3 Ud 1 Condensador electroltico 100F 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 tensin 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 metlica 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