Introducción al sensor de movimiento IMU (6 DOF)

Código

• Código Arduino para sensor MPU

Código Arduino para sensor MPU Arduino

 * /// I2Cdev y MPU6050 deben instalarse como bibliotecas, o de lo contrario los archivos .cpp / .h // para ambas clases deben estar en la ruta de inclusión de su proyecto # include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20 .h "// # incluir" MPU6050.h "// no es necesario si se utilizan MotionApps incluir archivo // Se requiere la biblioteca Arduino Wire si I2Cdev I2CDEV_ARDUINO_WIRE implementación // se usa en I2Cdev.h # si I2CDEV_IMPLEMENTATION ==I2CDEV_ARDUINO_WIRE #include" Wire .h "# endif // la dirección I2C predeterminada de la clase es 0x68 // direcciones I2C específicas se pueden pasar como parámetro aquí // AD0 low =0x68 (predeterminado para la placa de evaluación SparkFun y la placa de evaluación InvenSense) // AD0 high =0x69MPU6050 mpu; / / MPU6050 mpu (0x69); // <- usar para AD0 alto / * =======================================================================NOTA:Además de la conexión 3.3v, GND, SDA y SCL, este boceto depende de que el pin INT del MPU-6050 esté conectado al pin de interrupción externa # 0 del Arduino. En Arduino Uno y Mega 2560, este es el pin 2 de E / S digital. * =========================================================================* // * =======================================================================NOTA:Arduino v1.0.1 con la placa Leonardo genera un error de compilación cuando se usa Serial.write (buf, len). La salida de Teapot utiliza este método. La solución requiere una modificación en el archivo Arduino USBAPI.h, que afortunadamente es simple, pero molesto. Esto se solucionará en la próxima versión de IDE. Para obtener más información, consulte estos enlaces:http://arduino.cc/forum/index.php/topic,109987.0.html http://code.google.com/p/arduino/issues/detail?id=958 * =======================================================================* /// descomente "OUTPUT_READABLE_QUATERNION" si desea ver los // componentes reales del cuaternión en un [w, x, y, z] formato (no es el mejor para analizar // en un host remoto como Processing o algo así) // # define OUTPUT_READABLE_QUATERNION // descomenta "OUTPUT_READABLE_EULER" si quieres ver los ángulos de Euler // (en grados) calculados a partir de los cuaterniones que vienen desde el FIFO.// Tenga en cuenta que los ángulos de Euler sufren de bloqueo de cardán (para obtener más información, consulte // http://en.wikipedia.org/wiki/Gimbal_lock)//#define OUTPUT_READABLE_EULER // descomente "OUTPUT_READABLE_YAWPITCHROLL" si lo desea para ver los ángulos de guiñada /// cabeceo / balanceo (en grados) calculados a partir de los cuaterniones procedentes // del FIFO. Tenga en cuenta que esto también requiere cálculos de vector de gravedad .// También tenga en cuenta que los ángulos de guiñada / cabeceo / balanceo sufren de bloqueo del cardán (para // más información, consulte:http://en.wikipedia.org/wiki/Gimbal_lock)#define OUTPUT_READABLE_YAWPITCHROLL / / descomenta "OUTPUT_READABLE_REALACCEL" si quieres ver componentes de aceleración // sin gravedad. Este marco de referencia de aceleración // no está compensado por la orientación, por lo que + X es siempre + X según el // sensor, solo que sin los efectos de la gravedad. Si desea que la aceleración // compense la orientación, use OUTPUT_READABLE_WORLDACCEL en su lugar .//#define OUTPUT_READABLE_REALACCEL // descomente "OUTPUT_READABLE_WORLDACCEL" si desea ver la aceleración // componentes con gravedad eliminada y ajustada para el marco mundial de // referencia (la orientación es en relación con la orientación inicial, ya que en este caso no hay ningún magnetómetro //). Podría ser bastante útil en algunos casos.//#define OUTPUT_READABLE_WORLDACCEL // descomente "OUTPUT_TEAPOT" si desea una salida que coincida con el // formato utilizado para la demostración de la tetera de InvenSense // # defina OUTPUT_TEAPOT # define LED_PIN 13 // (Arduino es 13 , Teensy tiene 11 años, Teensy ++ tiene 6) bool blinkState =false; // Control / estado de MPU varsbool dmpReady =false; // establecer verdadero si DMP init fue exitosouint8_t mpuIntStatus; // contiene el byte de estado de interrupción real de MPUuint8_t devStatus; // devuelve el estado después de cada operación del dispositivo (0 =éxito,! 0 =error) uint16_t packetSize; // tamaño esperado del paquete DMP (el valor predeterminado es 42 bytes) uint16_t FIFoCount; // recuento de todos los bytes actualmente en FIFOuint8_t fifoBuffer [64]; // Búfer de almacenamiento FIFO // orientación / movimiento varsQuaternion q; // [w, x, y, z] contenedor de cuaternionesVectorInt16 aa; // [x, y, z] medidas del sensor de aceleración VectorInt16 aaReal; // [x, y, z] mediciones del sensor de aceleración sin gravedad VectorInt16 aaWorld; // [x, y, z] mediciones del sensor de aceleración de marco mundial VectorFloat gravity; // [x, y, z] vector de gravedad flotante euler [3]; // [psi, theta, phi] ángulo de Euler containerfloat ypr [3]; // [guiñada, cabeceo, balanceo] yaw / pitch / roll contenedor y vector de gravedad // estructura del paquete para la tetera InvenSense demouint8_t teapotPacket [14] ={'$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\ r', '\ n'}; // ================================================================// ===RUTINA DE DETECCIÓN DE INTERRUPCIONES ===// ==============================================================volatile bool mpuInterrupt =false; // indica si el pin de interrupción de MPU ha subido alto. dmpDataReady () {mpuInterrupt =true;} // ================================================================// ===CONFIGURACIÓN INICIAL ===// ==============================================================void setup () {// unirse al bus I2C (la biblioteca I2Cdev no hace esto automáticamente) #if I2CDEV_IMPLEMENTATION ==I2CDEV_ARDUINO_WIRE Wire.begin (); TWBR =24; // Reloj I2C de 400 kHz (200 kHz si la CPU es de 8 MHz) #elif I2CDEV_IMPLEMENTATION ==I2CDEV_BUILTIN_FASTWIRE Fastwire ::setup (400, true); #endif // inicializar la comunicación en serie // (se eligió 115200 porque es necesario para la salida de la demostración de Teapot, pero // realmente depende de usted según su proyecto) Serial.begin (115200); mientras (! Serial); // espere la enumeración de Leonardo, otros continúan inmediatamente // NOTA:Los procesadores host de 8MHz o más lentos, como Teensy @ 3.3v o Ardunio // Pro Mini que se ejecuta a 3.3v, no pueden manejar esta velocidad en baudios de manera confiable debido // a la sincronización en baudios estar demasiado desalineado con las garrapatas del procesador. Debe usar // 38400 o más lento en estos casos, o usar algún tipo de solución de cristal // separada externa para el temporizador UART. // inicializar el dispositivo Serial.println (F ("Inicializando dispositivos I2C ...")); mpu.initialize (); // verifica la conexión Serial.println (F ("Probando las conexiones del dispositivo ...")); Serial.println (mpu.testConnection ()? F ("Conexión de MPU6050 exitosa"):F ("Error de conexión de MPU6050")); // espera a que esté listo Serial.println (F ("\ nEnviar cualquier carácter para comenzar la programación y demostración DMP:")); while (Serial.available () &&Serial.read ()); // búfer vacío while (! Serial.available ()); // esperar datos while (Serial.available () &&Serial.read ()); // vaciar el búfer de nuevo // cargar y configurar el DMP Serial.println (F ("Inicializando DMP ...")); devStatus =mpu.dmpInitialize (); // proporcione aquí sus propias compensaciones giroscópicas, escaladas para sensibilidad mínima mpu.setXGyroOffset (220); mpu.setYGyroOffset (76); mpu.setZGyroOffset (-85); mpu.setZAccelOffset (1788); // 1688 predeterminado de fábrica para mi chip de prueba // asegúrese de que funcionó (devuelve 0 si es así) if (devStatus ==0) {// enciende el DMP, ahora que está listo Serial.println (F ("Habilitando DMP. .. ")); mpu.setDMPEnabled (verdadero); // habilita la detección de interrupciones de Arduino Serial.println (F ("Habilitando la detección de interrupciones (interrupción externa de Arduino 0) ...")); attachInterrupt (0, dmpDataReady, RISING); mpuIntStatus =mpu.getIntStatus (); // establece nuestro indicador DMP Ready para que la función main loop () sepa que está bien usarlo Serial.println (F ("DMP listo! Esperando la primera interrupción ...")); dmpReady =verdadero; // obtener el tamaño de paquete DMP esperado para una comparación posterior packetSize =mpu.dmpGetFIFOPacketSize (); } else {// ¡ERROR! // 1 =falló la carga de memoria inicial // 2 =fallaron las actualizaciones de la configuración DMP // (si se va a romper, normalmente el código será 1) Serial.print (F ("DMP Initialization failed (code")); Serial. print (devStatus); Serial.println (F (")")); } // configurar LED para salida pinMode (LED_PIN, OUTPUT);} // ================================================================// ===BUCLE PRINCIPAL DEL PROGRAMA ===// ==============================================================void loop () {// si la programación falló, no intente hacer nada if (! dmpReady) return; // espere la interrupción de MPU o paquetes adicionales disponibles mientras (! mpuInterrupt &&FIFoCount  1 paquete disponible // (esto nos permite leer más inmediatamente sin esperar una interrupción) FIFOCount - =packetSize; #ifdef OUTPUT_READABLE_QUATERNION // muestra los valores del cuaternión en forma de matriz sencilla:w x y z mpu.dmpGetQuaternion (&q, fifoBuffer); Serial.print ("quat \ t"); Serial.print (q.w); Serial.print ("\ t"); Serial.print (q.x); Serial.print ("\ t"); Serial.print (q.y); Serial.print ("\ t"); Serial.println (q.z); #endif #ifdef OUTPUT_READABLE_EULER // muestra los ángulos de Euler en grados mpu.dmpGetQuaternion (&q, fifoBuffer); mpu.dmpGetEuler (euler, &q); Serial.print ("euler \ t"); Serial.print (euler [0] * 180 / M_PI); Serial.print ("\ t"); Serial.print (euler [1] * 180 / M_PI); Serial.print ("\ t"); Serial.println (euler [2] * 180 / M_PI); #endif #ifdef OUTPUT_READABLE_YAWPITCHROLL // muestra los ángulos de Euler en grados mpu.dmpGetQuaternion (&q, fifoBuffer); mpu.dmpGetGravity (&gravedad, &q); mpu.dmpGetYawPitchRoll (ypr, &q y gravedad); Serial.print ("ypr \ t"); Serial.print (ypr [0] * 180 / M_PI); Serial.print ("\ t"); Serial.print (ypr [1] * 180 / M_PI); Serial.print ("\ t"); Serial.println (ypr [2] * 180 / M_PI); #endif #ifdef OUTPUT_READABLE_REALACCEL // muestra la aceleración real, ajustada para eliminar la gravedad mpu.dmpGetQuaternion (&q, fifoBuffer); mpu.dmpGetAccel (&aa, FIFoBuffer); mpu.dmpGetGravity (&gravedad, &q); mpu.dmpGetLinearAccel (&aaReal, &aa, &gravity); Serial.print ("areal \ t"); Serial.print (aaReal.x); Serial.print ("\ t"); Serial.print (aaReal.y); Serial.print ("\ t"); Serial.println (aaReal.z); #endif #ifdef OUTPUT_READABLE_WORLDACCEL // muestra la aceleración inicial del marco mundial, ajustada para eliminar la gravedad // y girada según la orientación conocida del cuaternión mpu.dmpGetQuaternion (&q, FIFoBuffer); mpu.dmpGetAccel (&aa, FIFoBuffer); mpu.dmpGetGravity (&gravedad, &q); mpu.dmpGetLinearAccel (&aaReal, &aa, &gravity); mpu.dmpGetLinearAccelInWorld (&aaWorld, &aaReal, &q); Serial.print ("mundo \ t"); Serial.print (aaWorld.x); Serial.print ("\ t"); Serial.print (aaWorld.y); Serial.print ("\ t"); Serial.println (aaWorld.z); #endif #ifdef OUTPUT_TEAPOT // muestra los valores de cuaternión en el formato de demostración de InvenSense Teapot:teapotPacket [2] =fifoBuffer [0]; teapotPacket [3] =FifoBuffer [1]; teapotPacket [4] =FifoBuffer [4]; teapotPacket [5] =FifoBuffer [5]; teapotPacket [6] =FifoBuffer [8]; teapotPacket [7] =FifoBuffer [9]; teapotPacket [8] =FifoBuffer [12]; teapotPacket [9] =FifoBuffer [13]; Serial.write (teapotPacket, 14); teapotPacket [11] ++; // packetCount, se repite en 0xFF a propósito #endif // parpadea el LED para indicar actividad blinkState =! blinkState; digitalWrite (LED_PIN, blinkState); }} 

Esquemas