maar

hello guys, i make project use 2 mpu-6050 at same time with arduino. i used example at https://github.com/jrowberg/i2cdevlib for my reference. here my coding modification for 2 mpu-6050 #include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif MPU6050 mpu(0x68); MPU6050 mpu2(0x69); // <-- use for AD0 high #define OUTPUT_READABLE_YAWPITCHROLL #define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards #define LED_PIN 13 bool blinkState = false; // MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t mpuIntStatus2; uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint8_t devStatus2; uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t packetSize2; uint16_t fifoCount; // count of all bytes currently in FIFO uint16_t fifoCount2; uint8_t fifoBuffer[64]; // FIFO storage buffer // orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity; // [x, y, z] gravity vector float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector float ypr2[3]; // packet structure for InvenSense teapot demo uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high void dmpDataReady() { mpuInterrupt = true; } void setup() { // join I2C bus (I2Cdev library doesn't do this automatically) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif // initialize serial communication // (115200 chosen because it is required for Teapot Demo output, but it's // really up to you depending on your project) Serial.begin(115200); while (!Serial); // wait for Leonardo enumeration, others continue immediately // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to // the baud timing being too misaligned with processor ticks. You must use // 38400 or slower in these cases, or use some kind of external separate // crystal solution for the UART timer. // initialize device Serial.println(F("Initializing I2C devices...")); mpu.initialize(); mpu2.initialize(); pinMode(INTERRUPT_PIN, INPUT); // verify connection Serial.println(F("Testing device connections...")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); Serial.println(mpu2.testConnection() ? F("MPU6050-2 connection successful") : F("MPU6050-2 connection failed")); // wait for ready Serial.println(F("\nSend any character to begin DMP programming and demo: ")); while (Serial.available() && Serial.read()); // empty buffer while (!Serial.available()); // wait for data while (Serial.available() && Serial.read()); // empty buffer again // load and configure the DMP Serial.println(F("Initializing DMP...")); devStatus = mpu.dmpInitialize(); devStatus2 = mpu2.dmpInitialize(); // supply your own gyro offsets here, scaled for min sensitivity mpu.setXGyroOffset(220); mpu.setYGyroOffset(76); mpu.setZGyroOffset(-85); mpu.setZAccelOffset(1788); // 1688 factory default for my test chip mpu2.setXGyroOffset(220); mpu2.setYGyroOffset(76); mpu2.setZGyroOffset(-85); mpu2.setZAccelOffset(1788); // make sure it worked (returns 0 if so) if (devStatus == 0 && devStatus2 == 0) { // turn on the DMP, now that it's ready Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true); mpu2.setDMPEnabled(true); // enable Arduino interrupt detection Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus(); mpuIntStatus2 = mpu2.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); packetSize2 = mpu2.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) Serial.print(F("DMP Initialization failed (code ")); Serial.print(devStatus); Serial.println(F(")")); } // configure LED for output pinMode(LED_PIN, OUTPUT); } void loop() { // wait for MPU interrupt or extra packet(s) available while (!mpuInterrupt && fifoCount < packetSize) { // other program behavior stuff here // . // . // . // if you are really paranoid you can frequently test in between other // stuff to see if mpuInterrupt is true, and if so, "break;" from the // while() loop to immediately process the MPU data // . // . // . } // reset interrupt flag and get INT_STATUS byte mpuInterrupt = false; mpuIntStatus = mpu.getIntStatus(); mpuIntStatus2 = mpu2.getIntStatus(); // get current FIFO count fifoCount = mpu.getFIFOCount(); fifoCount2 = mpu2.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus & 0x10) || fifoCount == 1024 && (mpuIntStatus2 & 0x10) || fifoCount2 == 1024) { // reset so we can continue cleanly mpu.resetFIFO(); mpu2.resetFIFO(); Serial.println(F("FIFO overflow!")); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (mpuIntStatus & 0x02 && mpuIntStatus2 & 0x02) { // wait for correct available data length, should be a VERY short wait while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount(); // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); mpu2.getFIFOBytes(fifoBuffer, packetSize); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= packetSize; fifoCount2 -= packetSize2; #ifdef OUTPUT_READABLE_YAWPITCHROLL // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); mpu2.dmpGetQuaternion(&q, fifoBuffer); mpu2.dmpGetGravity(&gravity, &q); mpu2.dmpGetYawPitchRoll(ypr2, &q, &gravity); 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.print(ypr[2] * 180/M_PI); Serial.print("\t"); Serial.print("ypr2\t"); Serial.print(ypr2[0] * 180/M_PI); Serial.print("\t"); Serial.print(ypr2[1] * 180/M_PI); Serial.print("\t"); Serial.println(ypr2[2] * 180/M_PI); #endif // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } } and i got problem when DMP waiting first interrupt then doing nothing, anyone can help me fix my coding?