Documentos de Académico
Documentos de Profesional
Documentos de Cultura
Objetivo General:
Objetivos Específicos:
Planteamiento
• Arduino NANO
• 3 Servo Motores
Servomotor TowerPro MG995 con alto Torque, hasta 15Kg-cm. Robusto, alto
rendimiento, compacto, con engranes de Metal, tamaño estándar, incluye accesorios y
tornillos para montarlo en cualquier proyecto. [5]
DIAGRAMA
+
Controlador Actuador Proceso
Setpoint -
Sensor
+
Arduino Servomotores Procesador de
Angulo de -
movimiento digital
Giro
Sensor
BNO055
En el siguiente diagrama tenemos una variable de setpoint la cual queremos regular que es el
ángulo de giro que seguiremos el siguiente proceso: como el controlador de nuestro prototipo
tenemos un Arduino UNO, el cual se encargara de controlar los 3 servos motores que serán los
actuadores de nuestro prototipo, el proceso estará controlado por un procesador de movimiento
digital que tendrá perturbaciones rápidas de giros, el sensor que utilizaremos es un BNO055 en el
cual estarán presentes un acelerómetro, manómetro y un giroscopio que nos ayudaran a regular
los giros de la gimbal.
Controlador
El MPU6050 IMU tiene acelerómetro de 3 ejes y giroscopio de 3 ejes integrados en un solo chip.
Por otro lado, el acelerómetro MPU6050 mide la aceleración de la misma manera que se
explicó en el video anterior para el sensor del acelerómetro ADXL345. Brevemente, puede medir
la aceleración gravitacional a lo largo de los 3 ejes y, mediante algunos cálculos de trigonometría,
podemos calcular el ángulo en el que se coloca el sensor. Por lo tanto, si fusionamos o
combinamos los datos del acelerómetro y el giroscopio, podemos obtener información muy
precisa sobre la orientación del sensor. [7]
La IMU MPU6050 también se denomina dispositivo de seguimiento de movimiento de seis
ejes o dispositivo de 6 DoF (seis grados de libertad), debido a sus 6 salidas, o las 3 salidas del
acelerómetro y las 3 salidas del giroscopio.
Arduino y MPU6050
Veamos cómo podemos conectar y leer los datos del sensor MPU6050 utilizando el Arduino.
Estamos utilizando el protocolo I2C para la comunicación con el Arduino, por lo que necesitamos
solo dos cables para conectarlo, más los dos cables para la alimentación.
Aquí está el código Arduino para leer los datos del sensor MPU6050. Debajo del código puedes
encontrar una descripción detallada del mismo.
1. #include <Wire.h>
2.
3. const int MPU = 0x68 ; // MPU6050 dirección I2C
4. flotar AccX, AccY, AccZ;
5. flotar GyroX, GyroY, GyroZ;
6. flotar accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
7. rollo de flotación , inclinación, desvío
8. flotar AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
9. float elapsedTime, currentTime, previousTime;
10. int c = 0 ;
11.
12. configuración vacía ( ) {
13. De serie. comienzo ( 19200 ) ;
14. Cable. comenzar ( ) ; // Inicializar comunicación
15. Cable. beginTransmission ( MPU ) ; // Iniciar la comunicación con MPU6050 // MPU = 0x68
16. Cable. escribir ( 0x6B ) ; // Habla con el registro 6B
17. Cable. escribir ( 0x00 ) ; // Restablecer - coloca un 0 en el registro 6B
18. Cable. endTransmission ( true ) ; // terminar la transmisión
19. / *
20. // Configurar la sensibilidad del acelerómetro - Rango de escala completa (valor predeterminado +/- 2g)
21. Wire.beginTransmission (MPU);
22. Wire.write (0x1C); // Habla con el registro ACCEL_CONFIG (1C hex)
23. Wire.write (0x10); // Establecer los bits de registro como 00010000 (+/- 8g rango de escala completa)
24. Wire.endTransmission (true);
25. // Configure Gyro Sensitivity - Full Scale Range (valor predeterminado +/- 250deg / s)
26. Wire.beginTransmission (MPU);
27. Wire.write (0x1B); // Habla con el registro GYRO_CONFIG (1B hex)
28. Wire.write (0x10); // Establezca los bits de registro como 00010000 (escala de 1000deg / s)
29. Wire.endTransmission (true);
30. retraso (20);
31. * /
32. // Llame a esta función si necesita obtener los valores de error de IMU para su módulo
33. calcular_IMU_error ( ) ;
34. retraso ( 20 ) ;
35. 36. } 37.
38. bucle de vacío ( ) {
39. // === Leer datos del acelerador === //
40. Cable. beginTransmission ( MPU ) ;
41. Cable. escribir ( 0x3B ) ; // Comenzar con el registro 0x3B (ACCEL_XOUT_H)
42. Cable. endTransmission ( false ) ;
43. Cable. requestFrom ( MPU, 6 , true ) ; // Leer 6 registros en total, cada valor de eje se almacena en 2
registros
44. // Para un rango de + -2g, necesitamos dividir los valores sin procesar por 16384, de acuerdo con la hoja de
datos
45. AccX = ( . Wire leer ( ) << 8 . | Alambre leer ( ) ) / 16384.0 ; // valor del eje X
46. Accy = ( . Wire leer ( ) << 8 . | Alambre leer ( ) ) / 16384.0 ; // valor del eje Y
47. AccZ = ( . Wire leer ( ) << 8 . | Alambre leer ( ) ) / 16384.0 ; // valor del eje Z
48. // Calculando Roll and Pitch a partir de los datos del acelerómetro
49. accAngleX = ( atan ( AccY / sqrt ( pow ( AccX, 2 ) + pow ( AccZ, 2 ) ) ) * 180 / PI ) - 0.58 ; // AccErrorX ~
(0.58) Consulte la función personalizada calcula_IMU_error () para obtener más detalles
50. accAngleY = ( atan ( - 1 * AccX / sqrt ( pow ( AccY, 2 ) + pow ( AccZ, 2 ) ) ) * 180 / PI ) + 1.58 ; //
AccErrorY ~ (-1.58)
51.
52. // === Leer datos del giroscopio === //
53. previousTime = currentTime; // El tiempo anterior se almacena antes del tiempo real leído
54. currentTime = millis ( ) ; // hora actual hora actual lectura
55. elapsedTime = ( currentTime - previousTime ) / 1000 ; // Divide por 1000 para obtener segundos
56. Cable. beginTransmission ( MPU ) ;
57. Cable. escribir ( 0x43 ) ; // Dirección de primer registro de datos Gyro 0x43
58. Cable. endTransmission ( false ) ;
59. Cable. requestFrom ( MPU, 6 , true ) ; // Leer 4 registros en total, cada valor de eje se almacena en 2
registros
60. GyroX = ( . Wire leer ( ) << 8 . | Alambre leer ( ) ) / 131,0 ; // Para un rango de 250deg / s tenemos que
dividir primero el valor bruto por 131.0, de acuerdo con la hoja de datos
61. GyroY = ( . Wire leer ( ) << 8 . | Alambre leer ( ) ) / 131,0 ; 62. GyroZ = ( . Wire leer ( ) << 8 . | Alambre
leer ( ) ) / 131,0 ;
63. // Corregir las salidas con los valores de error calculados.
64. GyroX = GyroX + 0,56 ; // GyroErrorX ~ (-0.56)
65. GyroY = GyroY - 2 ; // GyroErrorY ~ (2)
66. GyroZ = GyroZ + 0.79 ; // GyroErrorZ ~ (-0.8)
67.
68. // Actualmente los valores sin procesar están en grados por segundo, grados / s, por lo que necesitamos
multiplicar por sendonds (s) para obtener el ángulo en grados
69. gyroAngleX = gyroAngleX + GyroX * elapsedTime; // grado / s * s = grado
70. gyroAngleY = gyroAngleY + GyroY * elapsedTime;
71. yaw = yaw + GyroZ * elapsedTime;
72.
73. // Filtro complementario - combina valores de acelerómero y ángulo de giro
74. roll = 0.96 * gyroAngleX + 0.04 * accAngleX; 75. pitch = 0.96 * gyroAngleY
+ 0.04 * accAngleY;
76.
77. // Imprimir los valores en el monitor serie
78. De serie. impresión ( rollo ) ;
79. De serie. imprimir ( "/" ) ;
80. De serie. impresión ( tono ) ;
81. De serie. imprimir ( "/" ) ;
82. De serie. println ( guiñada ) ;
83. } 84.
85.
86. void calcula_IMU_error ( ) {
87. // Podemos llamar a esta función en la sección de configuración para calcular el error de datos del
acelerómetro y el giro. Desde aquí obtendremos los valores de error utilizados en las ecuaciones anteriores
impresas en el Monitor serie.
88. // Tenga en cuenta que debemos colocar la IMU plana para obtener los valores correctos, para que luego
podamos los valores correctos
89. // Leer los valores del acelerómetro 200 veces
90. mientras ( c < 200 ) {
91. Cable. beginTransmission ( MPU ) ;
92. Cable. escribir ( 0x3B ) ;
93. Cable. endTransmission ( false ) ;
94. Cable. requestFrom ( MPU, 6 , true ) ;
95. AccX = ( . Wire leer ( ) << 8 . | Alambre leer ( ) ) / 16384.0 ;
96. Accy = ( . Wire leer ( ) << 8 . | Alambre leer ( ) ) / 16384.0 ;
97. AccZ = ( . Wire leer ( ) << 8 . | Alambre leer ( ) ) / 16384.0 ;
98. // Suma todas las lecturas
99. AccErrorX = AccErrorX + ( ( atan ( ( AccY ) / sqrt ( pow ( ( AccX ) , 2 ) + pow ( ( AccZ ) , 2 ) ) ) * 180 /
PI
));
100. AccErrorY = AccErrorY + ( ( atan ( - 1 * ( AccX ) / sqrt ( pow ( ( AccY ) , 2 ) + pow ( ( AccZ ) , 2 ) ) ) *
180 / PI ) ) ;
101. c ++;
102. }
103. // Divide la suma por 200 para obtener el valor de error
104. AccErrorX = AccErrorX / 200 ;
105. AccErrorY = AccErrorY / 200 ;
106. c = 0 ;
107. // Leer valores de giroscopio 200 veces
108. mientras ( c < 200 ) {
109. Cable. beginTransmission ( MPU ) ;
110. Cable. escribir ( 0x43 ) ;
111. Cable. endTransmission ( false ) ;
112. Cable. requestFrom ( MPU, 6 , true ) ;
113. GyroX = Alambre. leer ( ) << 8 | Cable. leer ( ) ;
114. GyroY = Alambre. leer ( ) << 8 | Cable. leer ( ) ;
115. GyroZ = Alambre. leer ( ) << 8 | Cable. leer ( ) ;
116. // Suma todas las lecturas
117. GyroErrorX = GyroErrorX + ( GyroX / 131,0 ) ;
118. GyroErrorY = GyroErrorY + ( GyroY / 131,0 ) ;
Actuador
Los servos se admiten una tensión de alimentación entre 4,8V a 7,2V, siendo el valor más
adecuado es 6V. Con tensiones inferiores el motor tiene menos fuerza y velocidad. Con tensiones
superiores a 6,5V los servos empiezan a oscilar demasiado, lo cual los hace poco útiles. [8]
MG996R
Este es el servo de tamaño «grande» que usaremos frecuentemente. Este tipo de servos es
ampliamente utilizado en proyectos de robótica. Igual que con el SG90, muchos dispositivos y
kits como brazos robóticos, hexápodos, están diseñados para instalar este tamaño de servo. [8]
Las conexiones dependerán del tipo de servomotor con Arduino que estemos utilizando.
Todos deben tener 3 cables. Uno irá a tierra, otro a la alimentación de 5 Voltios y el tercero a
un pin PWM. También puedes utilizar un shield para controlar servomotores, como el de Adafruit.
Código
Esta acción ya nos ha hecho un include en nuestro código que nos permite controlar el servo.
Os dejo a continuación un código básico para posicionar el servo en los ángulos 0º, 90º y 180º.
[9]
8 void setup() {
10 Serial.begin(9600);
11
13 servoMotor.attach(9);
14 }
15
16 void loop() {
17
18 // Desplazamos a la posición 0º
19 servoMotor.write(0);
20 // Esperamos 1 segundo
21 delay(1000);
22
24 servoMotor.write(90);
25 // Esperamos 1 segundo
26 delay(1000);
27
29 servoMotor.write(180);
30 // Esperamos 1 segundo
31 delay(1000);
32 }
De la librería Servo.h hemos declarado un objeto o variable servoMotor y hacemos uso de dos
métodos. Por un lado, el attach, que nos permite indicar en que pin tenemos conectado nuestro
servo, y por otro lado el write, donde indicamos en qué ángulo queremos posicionar nuestro
servomotor. [9]
Tabla de error
ANEXOS
CÁLCULOS.
La tensión de alimentación depende del modelo del servomotor que se use. El servomotor
usado en nuestro proyecto comprende 4.5 V a 6 V.
La función de un servomotor es indicar la posición a través de una señal cuadrada de
voltaje. Su ángulo de ubicación depende del nivel alto de la señal.
Para el servomotor MG996R, los valores posibles de la señal están entre 1,0 ms / 60
grados, y 2,5 ms / 60 grados, que posicionan al motor en los extremos de giro.
Señal arrojada.
Referencias