Arduino en español
Circuitos con Arduino - Juan Antonio Villalpando
-- Tutorial de iniciación a Arduino --
Volver al índice del tutorial
____________________________
46A.- Giroscopio. Acelerómetro. Sensor de temperatura. MPU6050. (I).
- Tutoriales sobre el giroscopio:
46.- Giroscopio. Acelerómetro. MPU6050. I2C. (I)
46A.- Estudio del Giroscopio y Acelerómetro. MPU6050. I2C. (II)
46B.- Giroscopio. Acelerómetro. MPU6050. I2C. App Inventor. (III)
46C.- Giroscopio. Acelerómetro. MPU6050. I2C. App Inventor. Centro. (IV)
46D.- Giroscopio. Acelerómetro. MPU6050. I2C. App Inventor. Avión. (V)
--------------------------------------------------------------------------------------------------------------------
- Cuando has ejecutado aplicaciones con códigos del giroscopio habrás observamos una serie de números, que cambian rápidamente y que a veces no sabes interpretar.
- Este tutorial trata de aclarar algunas dudas sobre el giroscopio. No todas, solo algunas.
- Creo que es un tutorial bastante completo. Si te ha parecido bien, me lo indicas por correo.
_________________________________________
- Velocidad angular.
- Tenemos un punto rojo girando, según a la velocidad a la que vaya, podemos decir por ejemplo que recorre: 30º cada segundo.
- Esa sería su velocidad angular 30 º/s
- Es decir, tardaría 360 / 30 = 12 segundos en dar una vuelta. Lento.
- Ahora supongamos que lleva una velocidad angular de 720 º/ s
- La circunferecia tiene 360º, luego daría 2 vueltas por segundo. Rápido. |
|
__________________________________________
- Problemitas.
- Un elemento está girando a: 250º/s, ¿cuántas vueltas realiza en un minuto?
360º ----------- 1 vuelta
250º ----------- x vuetas
x = (250 * 1) / 360
En sesenta segudos girará (250 * 60) / 360 = 41,66 revoluciones por minuto (r.p.m.)
Es decir, que si tenemos un elemento cuyo valor máximo de escala es de 250 º/s, podemos medir velocidades de hasta 41,66 rpm.
- Comprueba los demás valores de esta tabla:
- Así que según queramos medir velocidades de giro rápidas o lentas, tomaremos una escala u otra, por lo general nuestro giroscopio viene configurado con una velocidad de 250º/s, es decir, que puede medir giros de hasta 41,66 rpm. Mediante configuración se puede establecer otros valores de la tabla anterior, pero con 250º/s nos valdrá en la mayoría de nuestras aplicaciones.
- También observamos cantidades de sensibilidad, en el caso de 250º/s es de 131. Deberemos dividir la velocidad obtenida entre ese número para obtener el valor correcto de velocidad. Eso se realiza en el código de Arduino.
- El signo * y -, simplemente indica que podemos medir en sentido horario o antihorario.
- Errores.
- Volviendo a 250º/s y sensibilidad de 131, si dividimos 250 / 131 = 2º
- Es decir, que el error de medida será de aproximadamente 2º/s.
- Si configuramos a 2000º/s y sensibilidad 16.4, dividimos 2000 / 16.4 = 122º
- El error de medida será de unos 122º/s
- Al disminuir la sensibilidad, el error cometido es grande.
_________________________________________
- Grados y radianes.
- Hay varias maneras de medir los ángulos, por ejemplo en grados y radianes.
- En el dibujo de la derecha observamos dos circunferencias cuyos ángulos están medidos en radianes y en grados.
- En grados una vuelta son 360 º
- En radianes una vuelta son 2*PI
- Para pasar de grados a radianes utilizamos:
- Para pasar de radianes a grados utilizamos:
|
|
_________________________________________
- Velocidad angular en radianes.
- La velocidad angular se suele expresar en radianes/segundos, por ejemplo 3 rad/s
_________________________________________
- El giroscopio.
- El giroscopio mide velocidades angulares, en la mayoría de los tutoriale se expresan en º/s.
- Se mide la velocidad en los tres ejes, X, Y y Z.
- Si ponemos un giroscopio sobre una mesa y lo giramos sobre su eje vertical (Z), obtendremos su velocidad angular Z.
_________________________________________
- El acelerómetro.
- Mide aceleraciones en m/s2
- Aceleración significa que está cambiando continuamente su velocidad.
- Ejemplo, un objeto va a 4 m/s, y 1 segundo después va más rápido, a 9 m/s.
- Ha acelerado (9 - 4) = 5 m/s2
- Un avión va a 600 km/h, un poco después se pone a una velocidad constate de 800 km/h ¿Qué aceleración tiene ahora?, pues NINGUNA porque si va constantemente a 800 km/h no va acelerando. En este caso no podemos saber cuándo ha acelerado desde 600 km/h a 800/h porque no nos indican en qué tiempo ha realizado ese cambio.
- Un objeto que va muy rápido, no quiere decir que vaya con mucha aceleración. La aceleración solo se produce cuando está cambiando la velocidad.
- En el planeta tierra existe la fuerza de la gravedad, unos g = 9,8 m/s2, es la fuerza con la que la tierra atrae a los cuerpos y siempre existe.
- Qué significa 2g, 4g, 8g, 16g. Resulta de multiplicar 2, 4, 8 o 16 por g g = 9,8 m/s2
- Nuestro dispositivo viene configurado de fábrica con un fondo de escala de 2g, es decir, puede medir aceleraciones de hasta 19,6 m/2, suficiente para la mayoría de los ejemplos.
- Se observa el factor de sensibilidad, si tenemos a nuestro acelerómetro configurado con 2g, deberemos dividir los valores obtenidos entre 16384.
- Si alguna vez has presenciado un espectáculo aeronaútico, recordarás que el locutor comenta que el piloto del avión F15 puede ir a 7G (68,6 m/s2).
- En una montaña rusa podemos estar en ciertos momentos a unos 3G.
- La aceleración se puede producir en los tres ejes X, Y y Z. La componente total de la aceleración se obtiene:
float v = sqrt(pow(ax,2) + pow(ay,2) + pow(az,2)) ;
- Errores.
- Volviendo a 2g y sensibilidad de 16384, si dividimos 19,6 / 16384 = 0,01g
- Es decir, que el error de medida será de aproximadamente 0,01g.
- Si configuramos a 16g y sensibilidad 2048, dividimos 156,8 / 2048 = 0,7g
- El error de medida será de unos 0,7g
- Al disminuir la sensibilidad, el error cometido es grande.
___________________________
- Módulo con el 6050.
- En la plaquita tenemos dos giroscopio (SDA-SCL) y (XDA-XSL), para que actue uno u otro, debemos poner la entrada AD0 a 0 o a 1. Si no conectamos nada a AD0, se establecerá a 0 y actuará (SDA-SCL), como observaremos en la mayoría de los tutoriales. Así que conectamos: Vcc, Gnd, SDA y SCL.
- El terminal INT es para enviar una interrupción al microcontrolador y que este le preste atención, es interesante, pero no lo ampliaré en este tutorial.
_______________________
- Programación.
- MPU-6050 Hoja de datos.
- MPU-6050
- Para realizar la programación lo primero será indicar cual de los dos giroscopio queremos configurar (SDA-SCL) o (XDA-XSL).
- El 0x68 es el SDA-SCL y el 0x69 el XDA-XCL. Cada dispositivo conectado al bus I2C debe tener una dirección distinta, nuestro módulo a utilizar sus terminales SDA-SCL, está configurado con la 0x68.
- Primero indicamos que vamos a configurar el elemento 0x68 del bus I2C. En vez de escribir 0x68 también lo podemos expresar en binario: 0b1101000
Wire.beginTransmission(0x68); // También se puede poner en binario
Wire.beginTransmission(0b1101000);
- Una vez indicado el dispositivo I2C que queremos configurar, debemos indicarle que no se duerma :) SLEEP
- Para ello vamos al registro 0x6B y lo ponemos a cero.
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission(); // Fin de esta parte de la configuración.
_____________________________________________________________
- Configuración de la escala de la aceleración en el acelerómetro. Registro 1C.
- Ahora vamos al registro 1C y le vamos a indicar que queremos configurar la aceleración AFS_SEL.
- La pondremos a 8g, miramos la tabla, arriba, en este tutorial y observamos que AFS_SEL debe ser 00010000, para la 8g.
- Hacemos esto:
//Configure the accelerometer (+/-8g)
Wire.beginTransmission(0x68); // Comienza la comunicación con el MPU-6050
Wire.write(0x1C); // Petición de escritura en ese registro.
Wire.write(0x10); // Escritura en el registro. Aquí elegimos los 8g
Wire.endTransmission();
- Si quisiéramos 2g, pondríamos (0x00)
- Es decir, debemos configuar los Bit4 y Bit3 de esta manera según la máxima aceleración que queramos medir
AFS_SEL |
Full Scale Range |
Bit4 |
Bit3 |
|
Wire.write( ) |
Sensibility |
0 |
2g |
0 |
0 |
00000000 |
(0x00) |
16384 |
1 |
4g |
0 |
1 |
00001000 |
(0x08) |
8192 |
2 |
8g |
1 |
0 |
00010000 |
(0x10) |
4096 |
3 |
16g |
1 |
1 |
00011000 |
(0x18) |
2048 |
_____________________________________________________________
- Vamos a configurar la escala de velocidad en el giroscopio. Registro 1B.
- Eso se hace en el registro 1B.
- Vamos a configurarlo con la escala de 500 º/s, miramos la tabla al principio de esta página y observamos que debemos poner 00001000
- [Si quisiéramos 250 º/s pondríamos 00000000]
- Es decir, debemos configuar los Bit4 y Bit3 de esta manera según la máxima aceleración que queramos medir
FS_SEL |
Full Scale Range |
Bit4 |
Bit3 |
|
Wire.write( ) |
Sensibility |
0 |
250 |
0 |
0 |
00000000 |
(0x00) |
131 |
1 |
500 |
0 |
1 |
00001000 |
(0x08) |
65.5 |
2 |
1000 |
1 |
0 |
00010000 |
(0x10) |
32.8 |
3 |
2000 |
1 |
1 |
00011000 |
(0x18) |
16.4 |
// Configure the gyro (500dps full scale)
Wire.beginTransmission(0x68); // Comienza la comunicación con el MPU-6050
Wire.write(0x1B); // Petición de escritura en ese registro.
Wire.write(0x08); // Escritura en el registro. Aquí elegimos los 500
Wire.endTransmission();
----------------------------------------------------------------------------------------------------------------------
- La configuración de aceleración y giroscopio no es preciso hacerla siempre, si ya con otro código anterior hubiéramos configurados estos parámetros, no hace falta volverlo hacer.
______________________________
- Obtención de los datos. Registro 3B y los siguientes.
- Los datos que va obteniendo el giroscopio y el acelerómetro están a partir de la dirección 3B, son 14 registros, desde el 3B al 48.
- Obtenemos los valores y los asignamos a variables:
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(0x68,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
- Ya tenemos en las variables los valores captados por el giroscopio, acelerómetro. Este módulo tiene, además un sensor de temperatura.
--------------------------------------------------------------------------------
- Obtenemos una serie de valores que resultan de tomar los dos bytes que forman cada variable, por ejemplo:
- ¿Qué significa esta línea?
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcX está declarado como entero al principio del código.
Wire.read()<<8 lee el byte superior (Hight)
Wire.read lee el byte inferior (Low)
- Los << desplazan los 8 bytes a la izquierda indicando que son los más significativos.
- La tubería vertical | une a los dos bytes
- Imaginemos que contiene los valores 11001111 y 00110011, entre los dos: 1100111100110011
- Esto es un número binario de 16 bits.
- Lo que obtenemos es ese número convertido a entero.
- Para reconocer su valor debemos dividirlo entre su sensibilidad. Según la escala elegida tendrá una sensibilidad, por ejemplo 83,33 o 78,4
- En este caso estaría configurada con 2g y 250
float az_m_s2 = az * (9.81/16384.0);
float gx_deg_s = gx * (250.0/32768.0);
________________________________________________________________________________
- Cómo puedo ver lo datos en bruto, sin dividir entre la sensibilidad.
- Fíjate que aquí no utilizo librería <MPU6050.h>, por eso debo obtener los valores con Wire.read.
- En los ejemplos siguientes pondré esa librería y será más fácil obtener los valores de esta manera:
sensor.getAcceleration(&ax, &ay, &az);
sensor.getRotation(&gx, &gy, &gz);
#include <Wire.h>
//Declaring some global variables
int gyro_x, gyro_y, gyro_z;
long acc_x, acc_y, acc_z, acc_total_vector;
int temperature;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
long loop_timer;
int lcd_loop_counter;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch_output, angle_roll_output;
void setup() {
Wire.begin();
Serial.begin(9600);
setup_mpu_6050_registers();
}
void loop(){
read_mpu_6050_data();
// Lee dos datos.
Serial.print(acc_x);
Serial.print("|");
Serial.print(acc_y);
Serial.print("\n"); // Fin de línea.
delay(500);
}
void read_mpu_6050_data(){ //Subroutine for reading the raw gyro and accelerometer data
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,14); // Lee 14 bytes.
while(Wire.available() < 14);
acc_x = Wire.read()<<8|Wire.read();
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temperature = Wire.read()<<8|Wire.read();
gyro_x = Wire.read()<<8|Wire.read();
gyro_y = Wire.read()<<8|Wire.read();
gyro_z = Wire.read()<<8|Wire.read();
}
void setup_mpu_6050_registers(){
//Activate the MPU-6050
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
//Configure the accelerometer (+/-8g)
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();
//Configure the gyro (500dps full scale)
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x08);
Wire.endTransmission();
}
|
________________________________________________________________________________
- ¿Cómo puedo consultar la aceleración y la velocidad que tiene configurado mi módulo?
- Vamos a realizar este ejercicio:
- Ponemos la aceleración a 16g en el registo (0x1C), es decir los Bit4 y Bit3 del registro (0x1C) a 11, es decir: 00011000, es el número 24 en decimal, es decir:
En binario: 00011000
En decimal: 24
En hexadecimal: (0x18)
#include <Wire.h>
int gravedad;
void setup() {
Wire.begin();
Serial.begin(9600);
// Establecemos la aceleracion a (+/-16g)
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x18); // 18 es en hexadecimal, en binario: 00011000, en decimal: 24
Wire.endTransmission();
}
void loop(){
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.endTransmission();
Wire.requestFrom(0x68,1);
gravedad = Wire.read();
Serial.print(gravedad);
Serial.print("\n"); // Fin de línea.
delay(500);
}
|
- En la parte del setup, establecemos en el registro (0x1C) el número hexadecimal (0x18), que es el 00011000
- En la parte del loop, leemos en el registro (0x1C) y obtendremos el número 24, que es el 00011000.
- Para obtener el valor que tiene configurado el giroscopio, leeríamos el registro (0x1B).
________________________________________________________________________________
- Quiero hacer una aplicación completa. Establecer escala de velocidad y aceleración y ver resultados reales.
- Vamos a establecer el fondo de escala de la velocidad en 500 º/s y la aceleración en 8g.
- En esta aplicación solo se muestra el vector aceleración:
- Si quieres observar las componentes X, Y y Z, desmarca los comentarios del Serial.
- Fíjate que aquí utilizo librería <MPU6050.h>, por eso obtengo los valores mediante:
sensor.getAcceleration(&ax, &ay, &az);
sensor.getRotation(&gx, &gy, &gz);
#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>
MPU6050 sensor;
// Valores RAW (sin procesar) del acelerometro y giroscopio en los ejes x,y,z
int ax, ay, az;
int gx, gy, gz;
void setup() {
Serial.begin(9600); // Iniciando Monitor Serie
Wire.begin(); // Iniciando I2C
sensor.initialize(); // Iniciando el sensor
if (sensor.testConnection()) Serial.println("Sensor iniciado.");
else Serial.println("Fallo inicio del sensor");
// Vamos a configuración del sensor
configurar_sensor();
}
void loop() {
// Leer las aceleraciones y velocidades angulares
sensor.getAcceleration(&ax, &ay, &az);
sensor.getRotation(&gx, &gy, &gz);
float ax_m_s2 = ax * (9.81/4096.0);
float ay_m_s2 = ay * (9.81/4096.0);
float az_m_s2 = az * (9.81/4096.0);
float gx_deg_s = gx / 65.5;
float gy_deg_s = gy / 65.5;
float gz_deg_s = gz / 65.5;
// Serial.print("aceleraciones en m/s2 velocidades en grados/s):\t");
// Serial.print(ax_m_s2); Serial.print("\t");
// Serial.print(ay_m_s2); Serial.print("\t");
// Serial.print(az_m_s2); Serial.print("\t");
// Serial.print(gx_deg_s); Serial.print("\t");
// Serial.print(gy_deg_s); Serial.print("\t");
// Serial.print(gz_deg_s); Serial.print("\t");
// Vector aceleración.
// La raiz de la suma de los componentes al cuadrado.
float v = sqrt(pow(ax_m_s2,2) + pow(ay_m_s2,2) + pow(az_m_s2,2)) ;
Serial.print(v);
Serial.print("\n");
delay(100);
}
void configurar_sensor(){
//Activa el MPU-6050
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
// Configura el acelerometro con (+/-8g)
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10); // 00010000 = 8g. Sensibilidad = 4096
Wire.endTransmission();
// Configura el giroscopio con (500º/s)
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x08); // 00001000 = 500º/s.Sensibilidad = 65.5
Wire.endTransmission();
}
|
________________________________________________________________________________
- Serial Plotter.
- Para ver la información, abrimos el Monitor Serie (Herramientas / Monitor Serie)
- Pero también lo podemos ver con el Serial Plotter (Herramientas / Serial Plotter)
- El Monitor Serie y el Plotter no pueden funcionar a la vez.
________________________________________________________________________________
- Prueba con el Serial Plotter.
- En el ejempo anterior configuramos el módulo 6050 con 8g, como indiqué al principio de esta página, podrá medir aceleraciones de hasta 78,4 m/s2.
- Mueve rápidamente el módulo y observa las alturas que obtienes en el Plotter, alturas máximas de hasta 120
- Ahora cambia en el código:
Wire.write(0x10); // 0001000 = 8g. Sensibilidad = 4096
- por:
Wire.write(0x00); // 0000000 = 2g. Sensibilidad = 16384
- cambia también 16384 en la parte de las variables de la aplicación anterior.
- Ahora mueve rápidamente el módulo, observará que ahora se obtienen alturas máximas de hasta 36.
________________________________________________________________________________
- Los ángulos.
- En la mayoría de los tutoriales habrás observado que obtienen los ángulos con expresiones como esta, basada en trigonometría:
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));
//Calculate the total accelerometer vector //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
- Aquí lo vamos a calcular de otra forma. En inglés se denomina Pitch y Roll, pero en español es palante, patrás, unlao y otrolao.
- Haremos bisagra con el módulo MPU-6050 en esos 4 ejes. Es decir, apoyamos sobre un eje y giramos 90º.
- Fíjate que la orientación, es decir girar el módulo sobre el eje Z vertical no se suele hacer, esto más bien lo realiza una brújula.
- Bisagras:
- Vamos a ver qué sale...
- Vamos a obtener la aceleraciones ax y ay, directas, sin dividirla entre la sensibilidad.
Serial.print(ax); Serial.print("\t");
Serial.print(ay); Serial.print("\t");
#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>
MPU6050 sensor;
// Valores RAW (sin procesar) del acelerometro y giroscopio en los ejes x,y,z
int ax, ay, az;
int gx, gy, gz;
void setup() {
Serial.begin(9600); // Iniciando Monitor Serie
Wire.begin(); // Iniciando I2C
sensor.initialize(); // Iniciando el sensor
if (sensor.testConnection()) Serial.println("Sensor iniciado.");
else Serial.println("Fallo inicio del sensor");
// Vamos a configuración del sensor
configurar_sensor();
}
void loop() {
// Leer las aceleraciones y velocidades angulares
sensor.getAcceleration(&ax, &ay, &az);
sensor.getRotation(&gx, &gy, &gz);
float ax_m_s2 = ax * (9.81/4096.0);
float ay_m_s2 = ay * (9.81/4096.0);
float az_m_s2 = az * (9.81/4096.0);
float gx_deg_s = gx / 65.5;
float gy_deg_s = gy / 65.5;
float gz_deg_s = gz / 65.5;
// Serial.print("aceleraciones en m/s2 velocidades en grados/s):\t");
Serial.print(ax); Serial.print("\t");
Serial.print(ay); Serial.print("\t");
// Serial.print(az_m_s2); Serial.print("\t");
// Serial.print(gx_deg_s); Serial.print("\t");
// Serial.print(gy_deg_s); Serial.print("\t");
// Serial.print(gz_deg_s); Serial.print("\t");
// Vector aceleración.
// La raiz de la suma de los componentes al cuadrado.
// float v = sqrt(pow(ax_m_s2,2) + pow(ay_m_s2,2) + pow(az_m_s2,2)) ;
// Serial.print(v);
// Serial.print("\n");
delay(100);
}
void configurar_sensor(){
// Activa el MPU-6050
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
// Configura el acelerometro con (+/-8g)
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10); // 0001000 = 8g. Sensibilidad = 4096
Wire.endTransmission();
// Configura el giroscopio con (500º/s)
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x08); // 000010000 = 500º/s.Sensibilidad = 65.5
Wire.endTransmission();
}
|
- Obtenemos números aproximadamente entre 4100 y - 4100 cuando le hacemos bisagra a 90º.
- Ahora utilizamos la función map de Arduino, esto lo que hace es cambiar valores, cuando está totalmente vertical marcará aproximadamente 4100, hacemos corresponder ese númro a 90 mediante la función map.
x = map(ax, -4100, 4100, -90,90);
y = map(ay, -4100, 4100, -90,90);
- Si lo hubiéramos configurado con 2g, el número sería 16600.
#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>
MPU6050 sensor;
// Valores RAW (sin procesar) del acelerometro y giroscopio en los ejes x,y,z
int ax, ay, az;
int gx, gy, gz;
int x, y;
void setup() {
Serial.begin(9600); // Iniciando Monitor Serie
Wire.begin(); // Iniciando I2C
sensor.initialize(); // Iniciando el sensor
if (sensor.testConnection()) Serial.println("Sensor iniciado.");
else Serial.println("Fallo inicio del sensor");
// Vamos a configuración del sensor
configurar_sensor();
}
void loop() {
// Leer las aceleraciones y velocidades angulares
sensor.getAcceleration(&ax, &ay, &az);
sensor.getRotation(&gx, &gy, &gz);
float ax_m_s2 = ax * (9.81/4096.0);
float ay_m_s2 = ay * (9.81/4096.0);
float az_m_s2 = az * (9.81/4096.0);
float gx_deg_s = gx / 65.5;
float gy_deg_s = gy / 65.5;
float gz_deg_s = gz / 65.5;
// Serial.print("aceleraciones en m/s2 velocidades en grados/s):\t");
// Serial.print(ax); Serial.print("\t");
// Serial.println(ay); Serial.print("\t");
// Serial.print(az_m_s2); Serial.print("\t");
// Serial.print(gx_deg_s); Serial.print("\t");
// Serial.print(gy_deg_s); Serial.print("\t");
// Serial.print(gz_deg_s); Serial.print("\t");
x = map(ax, -4100, 4100, -90,90);
y = map(ay, -4100, 4100, -90,90);
Serial.print(x);
Serial.print("|");
Serial.print(y);
Serial.print("\n");
// Vector aceleración.
// La raiz de la suma de los componentes al cuadrado.
// float v = sqrt(pow(ax_m_s2,2) + pow(ay_m_s2,2) + pow(az_m_s2,2)) ;
// Serial.print(v);
// Serial.print("\n");
delay(100);
}
void configurar_sensor(){
// Activa el MPU-6050
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
// Configura el acelerometro con (8g)
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10); // 00010000 = 8g. Sensibilidad = 4096
Wire.endTransmission();
// Configura el giroscopio con (500º/s)
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x08); // 00001000 = 500º/s. Sensibilidad = 65.5
Wire.endTransmission();
}
|
________________________________________________________________________________
- Otras cosas.
- Han quedado varias cosas en el tintero:
1.- Realizar una autocalibración antes de comenzar como se ve en este ejemplo.
Código |
///////////////////////////////////////////////////////////////////////////////////////
/*Terms of use
///////////////////////////////////////////////////////////////////////////////////////
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//THE SOFTWARE.
///////////////////////////////////////////////////////////////////////////////////////
//Support
///////////////////////////////////////////////////////////////////////////////////////
Website: http://www.brokking.net/imu.html
Youtube: https://youtu.be/4BoIE8YQwM8
Version: 1.0 (May 5, 2016)
///////////////////////////////////////////////////////////////////////////////////////
//Connections
///////////////////////////////////////////////////////////////////////////////////////
Power (5V) is provided to the Arduino pro mini by the FTDI programmer
Gyro - Arduino pro mini
VCC - 5V
GND - GND
SDA - A4
SCL - A5
LCD - Arduino pro mini
VCC - 5V
GND - GND
SDA - A4
SCL - A5
*//////////////////////////////////////////////////////////////////////////////////////
//Include LCD and I2C library
#include <LiquidCrystal_I2C.h>
#include <Wire.h>
// LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
//Declaring some global variables
int gyro_x, gyro_y, gyro_z;
long acc_x, acc_y, acc_z, acc_total_vector;
int temperature;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
long loop_timer;
int lcd_loop_counter;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch_output, angle_roll_output;
//Initialize the LCD library
//LiquidCrystal_I2C lcd(0x27,16,2);
void setup() {
Wire.begin(); //Start I2C as master
//Serial.begin(57600); //Use only for debugging
pinMode(13, OUTPUT); //Set output 13 (LED) as output
setup_mpu_6050_registers(); //Setup the registers of the MPU-6050 (500dfs / +/-8g) and start the gyro
digitalWrite(13, HIGH); //Set digital output 13 high to indicate startup
lcd.begin(16,2); //Initialize the LCD
lcd.backlight(); //Activate backlight
lcd.clear(); //Clear the LCD
lcd.setCursor(0,0); //Set the LCD cursor to position to position 0,0
lcd.print(" MPU-6050 IMU"); //Print text to screen
lcd.setCursor(0,1); //Set the LCD cursor to position to position 0,1
lcd.print(" V1.0"); //Print text to screen
delay(1500); //Delay 1.5 second to display the text
lcd.clear(); //Clear the LCD
lcd.setCursor(0,0); //Set the LCD cursor to position to position 0,0
lcd.print("Calibrating gyro"); //Print text to screen
lcd.setCursor(0,1); //Set the LCD cursor to position to position 0,1
for (int cal_int = 0; cal_int < 2000 ; cal_int ++){ //Run this code 2000 times
if(cal_int % 125 == 0)lcd.print("."); //Print a dot on the LCD every 125 readings
read_mpu_6050_data(); //Read the raw acc and gyro data from the MPU-6050
gyro_x_cal += gyro_x; //Add the gyro x-axis offset to the gyro_x_cal variable
gyro_y_cal += gyro_y; //Add the gyro y-axis offset to the gyro_y_cal variable
gyro_z_cal += gyro_z; //Add the gyro z-axis offset to the gyro_z_cal variable
delay(3); //Delay 3us to simulate the 250Hz program loop
}
gyro_x_cal /= 2000; //Divide the gyro_x_cal variable by 2000 to get the avarage offset
gyro_y_cal /= 2000; //Divide the gyro_y_cal variable by 2000 to get the avarage offset
gyro_z_cal /= 2000; //Divide the gyro_z_cal variable by 2000 to get the avarage offset
lcd.clear(); //Clear the LCD
lcd.setCursor(0,0); //Set the LCD cursor to position to position 0,0
lcd.print("Pitch:"); //Print text to screen
lcd.setCursor(0,1); //Set the LCD cursor to position to position 0,1
lcd.print("Roll :"); //Print text to screen
digitalWrite(13, LOW); //All done, turn the LED off
loop_timer = micros(); //Reset the loop timer
}
void loop(){
read_mpu_6050_data(); //Read the raw acc and gyro data from the MPU-6050
gyro_x -= gyro_x_cal; //Subtract the offset calibration value from the raw gyro_x value
gyro_y -= gyro_y_cal; //Subtract the offset calibration value from the raw gyro_y value
gyro_z -= gyro_z_cal; //Subtract the offset calibration value from the raw gyro_z value
//Gyro angle calculations
//0.0000611 = 1 / (250Hz / 65.5)
angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
//Place the MPU-6050 spirit level and note the values in the following two lines for calibration
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll
if(set_gyro_angles){ //If the IMU is already started
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle
}
else{ //At first start
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
set_gyro_angles = true; //Set the IMU started flag
}
//To dampen the pitch and roll angles a complementary filter is used
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value
write_LCD(); //Write the roll and pitch values to the LCD display
while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
loop_timer = micros(); //Reset the loop timer
}
void read_mpu_6050_data(){ //Subroutine for reading the raw gyro and accelerometer data
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x3B); //Send the requested starting register
Wire.endTransmission(); //End the transmission
Wire.requestFrom(0x68,14); //Request 14 bytes from the MPU-6050
while(Wire.available() < 14); //Wait until all the bytes are received
acc_x = Wire.read()<<8|Wire.read(); //Add the low and high byte to the acc_x variable
acc_y = Wire.read()<<8|Wire.read(); //Add the low and high byte to the acc_y variable
acc_z = Wire.read()<<8|Wire.read(); //Add the low and high byte to the acc_z variable
temperature = Wire.read()<<8|Wire.read(); //Add the low and high byte to the temperature variable
gyro_x = Wire.read()<<8|Wire.read(); //Add the low and high byte to the gyro_x variable
gyro_y = Wire.read()<<8|Wire.read(); //Add the low and high byte to the gyro_y variable
gyro_z = Wire.read()<<8|Wire.read(); //Add the low and high byte to the gyro_z variable
}
void write_LCD(){ //Subroutine for writing the LCD
//To get a 250Hz program loop (4us) it's only possible to write one character per loop
//Writing multiple characters is taking to much time
if(lcd_loop_counter == 14)lcd_loop_counter = 0; //Reset the counter after 14 characters
lcd_loop_counter ++; //Increase the counter
if(lcd_loop_counter == 1){
angle_pitch_buffer = angle_pitch_output * 10; //Buffer the pitch angle because it will change
lcd.setCursor(6,0); //Set the LCD cursor to position to position 0,0
}
if(lcd_loop_counter == 2){
if(angle_pitch_buffer < 0)lcd.print("-"); //Print - if value is negative
else lcd.print("+"); //Print + if value is negative
}
if(lcd_loop_counter == 3)lcd.print(abs(angle_pitch_buffer)/1000); //Print first number
if(lcd_loop_counter == 4)lcd.print((abs(angle_pitch_buffer)/100)%10);//Print second number
if(lcd_loop_counter == 5)lcd.print((abs(angle_pitch_buffer)/10)%10); //Print third number
if(lcd_loop_counter == 6)lcd.print("."); //Print decimal point
if(lcd_loop_counter == 7)lcd.print(abs(angle_pitch_buffer)%10); //Print decimal number
if(lcd_loop_counter == 8){
angle_roll_buffer = angle_roll_output * 10;
lcd.setCursor(6,1);
}
if(lcd_loop_counter == 9){
if(angle_roll_buffer < 0)lcd.print("-"); //Print - if value is negative
else lcd.print("+"); //Print + if value is negative
}
if(lcd_loop_counter == 10)lcd.print(abs(angle_roll_buffer)/1000); //Print first number
if(lcd_loop_counter == 11)lcd.print((abs(angle_roll_buffer)/100)%10);//Print second number
if(lcd_loop_counter == 12)lcd.print((abs(angle_roll_buffer)/10)%10); //Print third number
if(lcd_loop_counter == 13)lcd.print("."); //Print decimal point
if(lcd_loop_counter == 14)lcd.print(abs(angle_roll_buffer)%10); //Print decimal number
}
void setup_mpu_6050_registers(){
//Activate the MPU-6050
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x6B); //Send the requested starting register
Wire.write(0x00); //Set the requested starting register
Wire.endTransmission(); //End the transmission
//Configure the accelerometer (+/-8g)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1C); //Send the requested starting register
Wire.write(0x10); //Set the requested starting register
Wire.endTransmission(); //End the transmission
//Configure the gyro (500dps full scale)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1B); //Send the requested starting register
Wire.write(0x08); //Set the requested starting register
Wire.endTransmission(); //End the transmission
} |
2.- Actuar con el terminal de INTERRUPCIONES INT.
3.- Filtrar la señal para que los valores no cambien con tanta rapidez.
En este código solo muestra cuando los valores han variado más de 3 puntos. Este código está realizado para 2g, por eso en el map tiene valores de unos 16600
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
// LiquidCrystal_I2C lcd(0x3F, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
int x, y, z;
int x_vieja, y_vieja;
void setup(){
lcd.begin(16,2);// Columnas y filas de LCD
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(9600);
}
void loop(){
Wire.beginTransmission(MPU);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
/**
Serial.print("AcX = "); Serial.print(AcX);
Serial.print(" | AcY = "); Serial.print(AcY);
Serial.print(" | AcZ = "); Serial.print(AcZ);
Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet
Serial.print(" | GyX = "); Serial.print(GyX);
Serial.print(" | GyY = "); Serial.print(GyY);
Serial.print(" | GyZ = "); Serial.println(GyZ);
*/
delay(20);
x=map(AcX, -15700,17100, -90,90);
y=map(AcY, -17000,15700, -90,90);
z=map(AcZ, -18000,15000, -90,90);
lcd.clear(); // Borra pantalla
lcd.setCursor(0,0); // Inicio del cursor
lcd.print(x); lcd.setCursor(8,0); lcd.print(y);
lcd.setCursor(0,1); // Siguiente renglón.
lcd.print(z);
if (x > (x_vieja + 3) || x < (x_vieja - 3))
{
Serial.print(x);
Serial.print("|");
Serial.print(y);
Serial.print("\n"); // Fin de línea. Importante.
x_vieja = x;
}
if (y > (y_vieja + 3) || y < (y_vieja - 3))
{
Serial.print(x);
Serial.print("|");
Serial.print(y);
Serial.print("\n"); // Fin de línea. Importante.
y_vieja = y;
}
}
|
________________________________
|