Jump to content
I2Cdevlib Forums

berat

Members
  • Posts

    1
  • Joined

  • Last visited

berat's Achievements

Newbie

Newbie (1/14)

0

Reputation

  1. Hi there, I am using MPU6050 with two servos. With some extra addings to example MPU6050_DMP6_using_DMP_V6.12 there was no problem with servos. But when I wanted to use BMP180 and combined all of these in one code I get FIFO overflow. I was looking for solutions but none of them solved my problem exactly(or can't apply correctly). I tried to minimize the readings of datas of BMP180 and I get FIFO overflow less frequently. But servos are still slow and respond the changes of MPU6050 litte lately. So this lagging absolutely ruins the project and I cannot make any progress without solving this problem. I would be grateful if anyone can help me. Here is my code: /* Arduino nano için; Bmp180 pinleri: sda -> a4 scl -> a5 vın -> 5v gnd -> gnd Mpu6050 pinleri: vcc -> 5v gnd -> gnd scl -> a5 sda -> a4 int -> d2 RGB Led pinleri: d9, d10, d11 Servo Motor pinleri: d7, d8 Buzzer pinleri: d12 */ #include <Wire.h> #include <Adafruit_BMP085.h> #include<Servo.h> #include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" //#include "MPU6050.h" // not necessary if using MotionApps include file #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif #include <Servo.h> Servo myservo,myservo2; // create servo object to control a servo #define seaLevelPressure_hPa 1013.25 Adafruit_BMP085 bmp; int R=9; int G=10; int B=11; const int buzzer = 12; MPU6050 mpu; // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual // quaternion components in a [w, x, y, z] format (not best for parsing // on a remote host such as Processing or something though) //#define OUTPUT_READABLE_QUATERNION // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles // (in degrees) calculated from the quaternions coming from the FIFO. // Note that Euler angles suffer from gimbal lock (for more info, see // http://en.wikipedia.org/wiki/Gimbal_lock) //#define OUTPUT_READABLE_EULER // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ // pitch/roll angles (in degrees) calculated from the quaternions coming // from the FIFO. Note this also requires gravity vector calculations. // Also note that yaw/pitch/roll angles suffer from gimbal lock (for // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) #define OUTPUT_READABLE_YAWPITCHROLL // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration // components with gravity removed. This acceleration reference frame is // not compensated for orientation, so +X is always +X according to the // sensor, just without the effects of gravity. If you want acceleration // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. //#define OUTPUT_READABLE_REALACCEL // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration // components with gravity removed and adjusted for the world frame of // reference (yaw is relative to initial orientation, since no magnetometer // is present in this case). Could be quite handy in some cases. //#define OUTPUT_READABLE_WORLDACCEL // uncomment "OUTPUT_TEAPOT" if you want output that matches the // format used for the InvenSense teapot demo //#define OUTPUT_TEAPOT #define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards #define LED_PIN 11 // (Arduino is 13, Teensy is 11, Teensy++ is 6) #define SERVO_PIN 7 // use pin 9 for servo control #define SERVO_PIN2 8 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 devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO 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 // packet structure for InvenSense teapot demo uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; // ================================================================ // === INTERRUPT DETECTION ROUTINE === // ================================================================ volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high void dmpDataReady() { mpuInterrupt = true; } // ================================================================ // === INITIAL SETUP === // ================================================================ void setup() { //Kırmızı renk elde etmek için... analogWrite(R,255); analogWrite(G,0); analogWrite(B,0); //delay(500); Serial.begin(38400); Serial.println("Ucus Bilgisayari deneme"); if (!bmp.begin()) { Serial.println("BMP280 bulunamadi!"); analogWrite(R,255); analogWrite(G,0); analogWrite(B,0); while (1) {} } myservo.attach(SERVO_PIN); myservo2.attach(SERVO_PIN2); myservo.write(0); // join I2C bus (I2Cdev library doesn't do this automatically) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); TWBR = 48; Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif // 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(); pinMode(INTERRUPT_PIN, INPUT); // verify connection Serial.println(F("Testing device connections...")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); if(!mpu.testConnection()){ analogWrite(R,255); analogWrite(G,0); analogWrite(B,0); } // 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(); // 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 // make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready Serial.println(F("Enabling DMP...")); mpu.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(); // 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(); } 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() { //Yeşil renk elde etmek için... analogWrite(R,0); analogWrite(G,255); analogWrite(B,0); //Serial.print("Sicaklik = "); Serial.print(bmp.readTemperature()); //Serial.println(" *C"); //Serial.print("Basinc = "); Serial.print(bmp.readPressure()); //Serial.println(" Pa"); //Serial.print("Yukseklik = "); Serial.print(bmp.readAltitude()); //Serial.println(" metre"); //Serial.print("Deniz seviyesinde basinc = "); Serial.print(bmp.readSealevelPressure()); //Serial.println(" Pa"); //Serial.print("Gercek Yukseklik = "); Serial.print(bmp.readAltitude(seaLevelPressure_hPa * 100)); //Serial.println(" metre"); Serial.println(""); // if programming failed, don't try to do anything if (!dmpReady) return; // 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(); // get current FIFO count fifoCount = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // reset so we can continue cleanly mpu.resetFIFO(); Serial.println(F("FIFO overflow!")); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (mpuIntStatus & 0x02) { // wait for correct available data length, should be a VERY short wait while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // read a packet from FIFO mpu.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; #ifdef OUTPUT_READABLE_QUATERNION // display quaternion values in easy matrix form: 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 // display Euler angles in degrees 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 // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); Serial.print("ypr\t"); Serial.print(ypr[0] * 180/M_PI); Serial.print("\t"); Serial.print(ypr[1] * 180/M_PI); //Output to servo myservo.write(180-(90+ ypr[2] * 180/M_PI)); myservo2.write(180-(90+ ypr[1] * 180/M_PI)); Serial.print("\t"); Serial.println(ypr[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_REALACCEL // display real acceleration, adjusted to remove gravity mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &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 // display initial world-frame acceleration, adjusted to remove gravity // and rotated based on known orientation from quaternion mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); Serial.print("aworld\t"); Serial.print(aaWorld.x); Serial.print("\t"); Serial.print(aaWorld.y); Serial.print("\t"); Serial.println(aaWorld.z); #endif #ifdef OUTPUT_TEAPOT // display quaternion values in InvenSense Teapot demo format: 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, loops at 0xFF on purpose #endif // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } }
×
×
  • Create New...