Extracting Data from MPU6050 (DESPERATE)

Hello everyone! I am a student and this community, Arduino, and programming are very new to me. I am currently working on a PID controlled self balancing robot for a presentation that I have in a couple weeks and could really use some help. For the bot, I used an UNO, H-bridge, and an MPU6050. I got the bot to balance and working just fine, but my mentor asked me to extract some data from the bot in order to create some graphs in excel for my presentation. I used the following code (which is not mine):

#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

#define MIN_ABS_SPEED 20

MPU6050 mpu;

// 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
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

//PID
double originalSetpoint = 173;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;

//adjust these values to fit your own design
double Kp = 60;   
double Kd = 1.4 ;
double Ki = 70;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.6;
//MOTOR CONTROLLER
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 8;
int IN4 = 9;
int ENB = 10;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

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();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif

mpu.initialize();

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
mpu.setDMPEnabled(true);

// enable Arduino interrupt detection
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;

// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();

//setup PID
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255); 
}
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(")"));
}
}


void loop()
{
// 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)
{
//no mpu data - performing PID calculations and output to motors 
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);

}

// 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;

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180/M_PI + 180;
}
}

From what I have read, you would use a Serial.print() command of some sort in order to print the data, I also think I know how to set it up for Excel already (using PLX.DAQ). My issue is I don’t know what to call in the above code or how to set it up in order to get the data off the chip. I have tried some methods that will compile but then the bot will not move or the wheels go haywire. I also tried this code (with and without the above code) to see if I could get readings and I did get readings in the below version:

 // MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>
const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(38400);
}
void loop(){
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,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(333);
}

Other methods I have looked into are using an SD card and a mount or using a bluetooth chip to upload to an android device. I would still be presented with the issue of what I need to call and set up in order to get the data off the bot. Anyway, because this all new, I am sure that I am missing something super obvious. If you guys could help me out, by perhaps sharing an example and explaining it, or telling me what to call, that would be great - I’m desperate and this was my last resort and I have been working on this for like 9 weeks.Thank you ahead of time.

Sincerely,

Stephanie

Sbravo34741:
.... to create some graphs in excel for my presentation....

if you need data just to get a static graph, would serial plotter not work for you? much less work! :slight_smile:

example on how to use here:

sherzaad:
if you need data just to get a static graph, would serial plotter not work for you? much less work! :slight_smile:

example on how to use here:
Using the Arduino Serial Plotter | Experimenter's Guide for Metro | Adafruit Learning System

Hello,

I’ve tried the serial plotter and I just can’t seem to get it up an running. I inserted the following code in the void setup section

int sensorPin = 2;
  int sensorValue = 0;

and this in void loop()

  pinMode(2, OUTPUT);
  int sensorValue = 0;
  Serial.begin(9600);

I get

Arduino: 1.8.5 (Windows 10), Board: "Arduino/Genuino Uno"

C:\Users\Valkyrie\Documents\Hernandez_REU\PID Bot\How_to_Make_Balancing_excel\How_to_Make_Balancing_excel.ino: In function 'void loop()':

How_to_Make_Balancing_excel:127: error: 'sensorValue' was not declared in this scope

 sensorValue = analogRead(2);

 ^

How_to_Make_Balancing_excel:128: error: 'SensorValue' was not declared in this scope

 Serial.println(SensorValue);

                ^

Multiple libraries were found for "PID_v1.h"
 Used: C:\Users\Valkyrie\Documents\Arduino\libraries\arduino-self-balancing-robot-master
 Not used: C:\Users\Valkyrie\Documents\Arduino\libraries\Arduino-PID-Library-master
 Not used: C:\Users\Valkyrie\Documents\Arduino\libraries\Arduino-PID-Library-master
 Not used: C:\Users\Valkyrie\Documents\Arduino\libraries\Arduino-PID-Library-master
 Not used: C:\Users\Valkyrie\Documents\Arduino\libraries\Arduino-PID-Library-master
exit status 1
'sensorValue' was not declared in this scope

This report would have more information with
"Show verbose output during compilation"
option enabled in File -> Preferences.

So, this is what I have so far: It compiles but now the bot won’t move at all…

//youtube.com/TARUNKUMARDAHAKE
//facebook.com/TARUNKUMARDAHAKE

#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"


#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
//++++++++++++++++++++++++++++++++++++++++++++++++++++++
int pos=0;
#define OUTPUT_READABLE_QUATERNION
#define OUTPUT_READABLE_YAWPITCHROLL
#define OUTPUT_READABLE_REALACCEL
#define LED_PIN 13
const int chipSelect = 4;

bool blinkState = false;
double timing;
double timing1;
//LMotorController motorController;

//++++++++++++++++++++++++++++++++++++++++++++++++++++++ 

#define MIN_ABS_SPEED 20

MPU6050 mpu;

// 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
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
//++++++++++++++++++++++++++++++++++++++++++++++++++++++

boolean start=0;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++ 
//PID
double originalSetpoint = 173;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;

//adjust these values to fit your own design
double Kp = 60;   
double Kd = 1.4 ;
double Ki = 70;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.6;
//MOTOR CONTROLLER
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 8;
int IN4 = 9;
int ENB = 10;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
mpuInterrupt = true;
}


void setup()
{
  //++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinMode(ENA, INPUT);
pinMode(IN1, INPUT);
pinMode(IN2, INPUT);
pinMode(IN3, INPUT);
pinMode(IN4, INPUT);
pinMode(ENB, INPUT);
//++++++++++++++++++++++++++++++++++++++++++++++++++++++ 
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();


TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
//++++++++++++++++++++++++++++++++++++++++++++++++++++++
Serial.begin(115200);
while (!Serial);
Serial.println(F("Initializing 12C devices..."));

//++++++++++++++++++++++++++++++++++++++++++++++++++++++ 
mpu.initialize();
//++++++++++++++++++++++++++++++++++++++++++++++++++++++
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 conncetion failed"));

//wait for ready

Serial.println(F("\nSend any character to begin DMP programming and demo:"));
while (Serial.available() && Serial.read());
while (!Serial.available());
while (Serial.available() && Serial.read());

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
mpu.setDMPEnabled(true);

// enable Arduino interrupt detection
attachInterrupt(0, 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();

//setup PID
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255); 
}
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(")"));



}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++
//pinMode(LED_PIN OUTPUT);
//++++++++++++++++++++++++++++++++++++++++++++++++++++++ 
}

int complete=0;
double timing_sec;
void loop()
{

  
// 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)
{
//no mpu data - performing PID calculations and output to motors 
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);

}

// 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 OUPUT_READABLE_QUATERNION
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.print(q.z);
#endif


#ifdef OUTPUT_READABLE_YAWPITCHROLL
/*  if (complete==0)
  motorController.write(20);

  if (complete==0)
  motorController.write(90);
*/  
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180/M_PI + 180;

Serial.print("ypr time\t");
Serial.print(ypr[0] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[3] * 180/M_PI);
Serial.print("\t");
timing = millis();
if (start == 0)
{ timing1=timing;
    start=1;
}
timing=timing-timing1;
timing_sec = timing/1000;
Serial.println(timing_sec);
if((timing_sec)/5.0 == 0.0) complete=1;
if((timing_sec)/5.0 == 0.0 && (timing_sec)/10.0 == 0.0) complete=0;
#endif

#ifdef OUTPUT_READBLE_REALACCEL
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

blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);

String dataString = "";
for (int count = 0; count < 3; count++) {
  dataString += String(ypr[count]);
  if (count < 3) {
    dataString += " ";
  }
}

//dataString += String(timing1/1000);

  }
}

Sbravo34741:
So, this is what I have so far: It compiles but now the bot won't move at all...

that's what usually happen when you try to mod someone else's code u don't fully understand! :slight_smile:

Ok.... go one step back to the code that actually worked balancing your bot.

now... what parameters in that code do you need to plot and how? (e.g y vs x , or x vs t , y vs t , ...etc)

sherzaad:
that's what usually happen when you try to mod someone else's code u don't fully understand! :slight_smile:

Ok.... go one step back to the code that actually worked balancing your bot.

now... what parameters in that code do you need to plot and how? (e.g y vs x , or x vs t , y vs t , ...etc)

You're right, it's not my code and I have been trying to understand it as much as possible (if anything this ordeal has helped me in that sense).
But your last question is what I have been racking my brain trying to figure out....

I have tried over and over again to figure out what to call - even my friends are stumped.

I know that if in the code I had written a function or int to call all six Accelerometer and gyroscope values, then I could do something like read the values and call them then print them to either the serial monitor or an SD card, bluetooth, etc.

But because this isn't my code, I can't for the life of me figure out what I need to call. Would it be in the MPU6050 library possibly? Am I using the wrong call? Maybe Serial.print doesn't work for what I am trying to call...I don't know but I haven't given up.

Sbravo34741:
You're right, it's not my code and I have been trying to understand it as much as possible (if anything this ordeal has helped me in that sense).
But your last question is what I have been racking my brain trying to figure out....

I have tried over and over again to figure out what to call - even my friends are stumped.

I know that if in the code I had written a function or int to call all six Accelerometer and gyroscope values, then I could do something like read the values and call them then print them to either the serial monitor or an SD card, bluetooth, etc.

But because this isn't my code, I can't for the life of me figure out what I need to call. Would it be in the MPU6050 library possibly? Am I using the wrong call? Maybe Serial.print doesn't work for what I am trying to call...I don't know but I haven't given up.

I'm not familiar with the library itself but looking at the first code you posted, shouldn't this give you the parameters you are looking for?

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

for trial, what happens if you add the following code just after those lines?

Serial.print("q.w=");
Serial.println(q.w);
Serial.print("q.x=");
Serial.println(q.x);
Serial.print("q.y=");
Serial.println(q.y);
Serial.print("q.z=");
Serial.println(q.z);
Serial.print("gravity.x=");
Serial.println(gravity.x);
Serial.print("gravity.y=");
Serial.println(gravity.y);
Serial.print("gravity.z=");
Serial.println(gravity.z);
Serial.print("yaw");
Serial.println("ypr[0]");
Serial.print("pitch");
Serial.println(ypr[1]);
Serial.print("roll");
Serial.println(ypr[2]);

sherzaad:
Hello,

Sorry I didn't come back for a while, I was working on the code with a friend of mine. We managed to get it to print things but again, wouldn't work...anyway...

I tried the code you provided (thank you by the way) - So, it's printing things out and the bot is moving but now the bot is going haywire. I will play around with the settings and give you an update as soon as I have something. Thank you so much for all the help and suggestions.