Velocidad angular.

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. - 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.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; } } -- M