Hi there. I am using an Arduino Leonardo. I got the GY521 breakout board, which has the MPU-6050. And I am also using the SG90 micro servo from TowerPro. 3 of them, really. Now my aim is to use yaw, pitch and roll to control the 3 servos (one servo for yaw, one for pitch, one for roll). Now I realised using that the MPU-6050 was not as easy as using an ADXL-3-something chip would've been (I assume you've all been there ;-;). So after intensive studying, I got the basics down for ypr, thanks to Jeff Rowberg. My code is a stripped down and slightly modified version of Jeff Rowberg's example code "MPU6050_DMP6", omitting parts non-essential to ypr values. I also included some code for Servo wizardry. HERE IS THE CODE:
/* SPECIAL THANKS TO JEFF ROWBERG
WIRING -
VCC - 5V
GND - GND
SCL - D3
SDA - D2
INT - 7
SERVOS - 9, 10, 11*/
#include "Servo.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h" // not necessary if using MotionApps include file
//#include "MPU6050.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation is used in I2Cdev.h
#include "Wire.h"
#endif
MPU6050 mpu;
#define MPU6050_INT_PIN 7
#define MPU6050_INT digitalPinToInterrupt(MPU6050_INT_PIN)
#define OUTPUT_READABLE_YAWPITCHROLL
#define LED_PIN 13
int p1; //servo position variables
int p2;
int p3;
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
Quaternion q; // [w, x, y, z] quaternion container
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
Servo s1;
Servo s2;
Servo s3;
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
s1.attach(9); //servo initialisation
s2.attach(10);
s3.attach(11);
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
Serial.println(F("Initializing I2C devices...")); // initialize device
mpu.initialize();
pinMode(MPU6050_INT_PIN, INPUT);
Serial.println(F("Testing device connections...")); // verify connection
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
Serial.println(F("\nSend any character to begin DMP programming and demo: ")); // wait for ready
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
Serial.println(F("Initializing DMP...")); // load and configure the 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.print("Enabling interrupt detection (Arduino external interrupt ");
Serial.print(MPU6050_INT); Serial.print(" on pin "); Serial.print(MPU6050_INT_PIN); Serial.println("...");
pinMode(MPU6050_INT, INPUT);
attachInterrupt(MPU6050_INT, dmpDataReady, RISING);
// 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);
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize) {}
mpuInterrupt = false; // reset interrupt flag and get INT_STATUS byte
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount(); // get current FIFO count
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
}
else if (mpuIntStatus & 0x02) {
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // wait for correct available data length, should be a VERY short wait
mpu.getFIFOBytes(fifoBuffer, packetSize); // read a packet from FIFO
fifoCount -= packetSize; //in case of > 1 packets
#ifdef OUTPUT_READABLE_YAWPITCHROLL
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);
Serial.print("\t");
Serial.println(ypr[2] * 180 / M_PI);
#endif
blinkState = !blinkState; // blink LED to indicate activity
digitalWrite(LED_PIN, blinkState);
}
//servo WIZARDRY
p1 = (ypr[0] * 180 / M_PI);
p1 = map(p1, -180, 180, 0, 150);
s1.write(p1);
p2 = (ypr[1] * 180 / M_PI);
p2 = map(p2, -90, 90, 0, 120);
s2.write(p2);
p3 = (ypr[2] * 180 / M_PI);
p3 = map(p3, -90, 90, 0, 120);
s3.write(p3);
}
Now it all works. Really it does. The problem: When I open the Serial Monitor and tell the Arduino to start the MPU-6050, the servos start. Sometimes, they stop and refuse to move unless I push them a bit. Sometimes, they don't move unless the reset the board. And most importantly, when I close the Serial Monitor, the servos don't move. HELP! Also, if you could show how I can jump right into the data without having to open the Serial Monitor, or even connect to my PC, only supplying power. And if you have any tips, modifications, improvements and/or suggestions, please let me know. Thanks in advance! (reply fast pls)