Jump to content
I2Cdevlib Forums

Another Arduino as external i2c device for MPU6050


Recommended Posts

Hello there,

I have built a balancing robot using the mpu6050 and have got it all to work well so far.

Here is a picture of the setup: https://drive.google.com/open?id=0B1X3D227-XxdSUVEekFsTXU0aXM

Now I am seeking to add some extra functionality to it, and implement a state space control scheme. I was originally using an Arduino Uno for everything, but now that I got encoders for the motors, and I have no more free interrupt pins on the main Uno board, I have also added a Pro-Mini to the setup. The Pro-Mini is intended to handle all the measurements and calculate the final values for the robot's horizontal position and vertical angle. With this setup, I would like to have the mpu6050, the Pro-Mini, and the Uno all connected via i2c, and only send or request data when the main Uno requires it during the loop. I got the setup working with a software serial connection between the two Arduino boards, but I wasn't happy with the speed of that setup. I've spent hours trying to get the Uno to act as a second slave device and respond to transmission events from the Pro-Mini, but I am pretty sure there are some conflicts with the DMP activities for the mpu6050.

 

I read this post: https://www.i2cdevlib.com/forums/topic/111-arduino-example-sketch-to-read-magnetometer-while-dmp-is-on/ and tried to adapt it to set the address to that set on my Uno, but still no luck.

 

This is the code I'm currently running on my Uno to be the receiver of the data that is sent:

#include <Wire.h>

void setup() {
  Wire.begin(0x1E);                // join i2c bus with address #8
  Wire.onReceive(receiveEvent); // register event
  Serial.begin(57600);           // start serial for output
}

void loop() {
  delay(100);
}

// function that executes whenever data is received from master
// this function is registered as an event, see setup()
void receiveEvent(int howMany) {
  while (1 < Wire.available()) { // loop through all but the last
    char c = Wire.read(); // receive byte as a character
    Serial.print(c);         // print the character
  }
  int x = Wire.read();    // receive byte as an integer
  Serial.println(x);         // print the integer
}

And this is what I last tried to upload to the Pro-Mini to attempt to see anything come through on the serial monitor of the Uno:

#define INTRO
//#define DEBUG
#define MINDEBUG
//#define INCLUDESOFTWARESERIAL
#define I2CDEV_SERIAL_DEBUG

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
//#include "SoftwareSerial.h"
#include <avr/wdt.h>
//#include <PinChangeInt.h>
  
// ================================================================
// === Define Timing Variables ===
// ================================================================
unsigned long loopTimeTotal = 0;
unsigned long loopTimeTop = 0;
double timeChange = 0;
  
// ================================================================
// === Define MPU 6050 Variables ===
// ================================================================
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
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
VectorInt16 aaOffset; // [x, y, z] accel sensor measurements offset
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
//float yprOffset[] = {0, 0.0604, 0}; // [yaw, pitch, roll] offset yaw/pitch/roll container and gravity vector
float yprOffset[] = {0, 0.04, 0}; //was 0.0805
int16_t gyros[3];      // [x, y, z] gyros container
float ang = 0.0; //angle in radians
float introAngle = 0.0; //used for intro conditions (>1.5 degrees)
float tempAngle = 0.0; //used for motor speed section
  
  // ================================================================
// === Define PID Variables ===
// ================================================================
//float Kp = 35.332; //original
float Kp = 19.79;
//float Ki = 331.852; //original
float Ki = 117.5682;
//float Kd = 0.824; // original
float Kd = .5429;


float err = 0.0; // current error container
float lasterr = 0.0; // previous error container
float errsum = 0.0; // integral of error
float derr = 0.0; // derivative of error
float desAng = 0.0; // desired angle
float dang = 0.0; //derivative of angle
float lastAng = 0.0; //previous angle container

double Vm = 0.0; //motor voltage desired
  
  
void setup() {
  Serial.begin(57600);
  mpuInitializeSetup();
  }
  
void loop() {
  loopTimeTop = millis();
  updateMeasPID();

  loopTimeTotal = millis() - loopTimeTop;
  timeChange = (double)(loopTimeTotal / 1000.0);
}
  
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// Update PID Control Variables
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void updateMeasPID(void) {
  lastAng = ang;
  //UPDATE MPU BASED VALUES
  fifoCount = mpu.getFIFOCount();
#ifdef DEBUG
  Serial.print("FIFO COUNT PID: ");
  Serial.println(fifoCount);
#endif
#ifdef MINDEBUG
  Serial.print("FIFO COUNT PID: ");
  Serial.println(fifoCount);
#endif
  while (fifoCount < packetSize) {
    fifoCount = mpu.getFIFOCount();
  }
  mpu.getFIFOBytes(fifoBuffer, packetSize);
  fifoCount -= packetSize;
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  mpu.dmpGetGyro(gyros, fifoBuffer);
  ang = ypr[1] - yprOffset[1]; //in radians


  dang = (ang - lastAng) / timeChange;
  //lasterr = err;
  err = desAng - ang;
  errsum += err * timeChange;
  errsum = constrain(errsum, -5, 5);
  //derr = (err - lasterr) / timeChange;

  //Update the desired voltage
  Vm = Kp * err + Ki * errsum - Kd * dang; //+Kd * derr;
#ifdef MINDEBUG
  Serial.print("\nAngle: ");
  Serial.println(ang * 180 / M_PI);
#endif

#ifdef DEBUG
  //Print out all the variables just updated
  Serial.print("\nAngle: ");
  Serial.print(ang * 180 / M_PI);
  Serial.print("\tError: ");
  Serial.print(err);
  Serial.print("\tErrsum: ");
  Serial.print(errsum);
  Serial.print("\tderr: ");
  Serial.println(derr);
  Serial.print("timeChange: ");
  Serial.print(timeChange);
  Serial.print("\tVm: ");
  Serial.println(Vm);
  Serial.print("\n");
#endif
}  
                                
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// mpu6050 Initialize Function
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void mpuInitializeSetup(void) {
  // Set up watchdog timer
  wdt_enable(WDTO_4S); //should be 4 seconds
  ///////////////////////////////////
  // initialize mpu6050///
  ///////////////////////////////////
  // initialize device
#ifdef INTRO
  Serial.println(F("Initializing I2C devices..."));
#endif
  mpu.initialize();
  // verify connection

  //Turn off watchdog timer if past this point
  wdt_disable();

#ifdef INTRO
  Serial.println(F("Testing device connections..."));
#endif
  if (mpu.testConnection()) {
#ifdef INTRO
    Serial.println(F("MPU6050 connection successful"));
#endif
  }
  else {
#ifdef INTRO
    Serial.println(F("MPU6050 connection failed"));
#endif
  }



  // load and configure the DMP
#ifdef INTRO
  Serial.println(F("Initializing DMP..."));
#endif
  devStatus = mpu.dmpInitialize();


    // set up in bypass mode
  mpu.setI2CMasterModeEnabled(0);
  mpu.setI2CBypassEnabled(1);

  Wire.beginTransmission(0x1E);
  Wire.write("Test");
  Wire.endTransmission();

  mpu.setI2CBypassEnabled(0);

  if (mpu.getI2CMasterModeEnabled()) {
#ifdef INTRO
    Serial.println(F("I2C Master Mode Enabled"));
#endif
  }
  else {
#ifdef INTRO
    Serial.println(F("I2C Master Mode Not Enabled"));
#endif
  }

  if (mpu.getI2CBypassEnabled()) {
#ifdef INTRO
    Serial.println(F("Bypass Mode Enabled"));
#endif
  }
  else {
#ifdef INTRO
    Serial.println(F("Bypass Mode Not Enabled"));
#endif
  }

  
  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
#ifdef INTRO
    Serial.println(F("Enabling DMP..."));
#endif
    mpu.setDMPEnabled(true);
    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();

    //delay(5000);
    //mpu.resetFIFO();

    //Initialize the angle to very high value so it trips the loop
    introAngle = 5;
    //Read data and wait for it to stabilize to between 1.5 degrees
    fifoCount = mpu.getFIFOCount();
    while (introAngle > 90) {
      //delay(20);
      if (fifoCount >= packetSize) {
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;
#ifdef DEBUG
        Serial.print("FIFO Count:\t");
        Serial.println(fifoCount);
#endif
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        ypr[1] -= yprOffset[1];
        introAngle = ypr[1] * 180 / M_PI; // degrees
        if (introAngle < 0) {
          introAngle = -introAngle;
        }
#ifdef DEBUG
        Serial.print("Angle:\t");
        Serial.println(introAngle);
#endif
      }
      else {
        while (fifoCount < packetSize) {
          fifoCount = mpu.getFIFOCount();
#ifdef DEBUG
          Serial.print("fifoCount-While:\t");
          Serial.println(fifoCount);
#endif
        } //while
      } //else
    } // main while loop
    //set angle
    //Serial.print("\nSetup While Exit Value: ");
    //Serial.println(introAngle);
    ang = ypr[1];
  } // if (devStatus == 0)
  else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
#ifdef INTRO
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
#endif
  }
}

 

Now, the main change I added to try and test if it was working is this part that is found in the mpuInitializeSetup() function at the end of my code:

    // set up in bypass mode
  mpu.setI2CMasterModeEnabled(0);
  mpu.setI2CBypassEnabled(1);

  Wire.beginTransmission(0x1E);
  Wire.write("Test");
  Wire.endTransmission();

  mpu.setI2CBypassEnabled(0);

I was hoping that by doing this, I would see the words "Test" on the serial monitor of the Uno. 

 

For the physical connections, I have the SCL and SDA lines of the Uno connected to the XCL and XDA lines of the mpu6050. The SCL and SDA lines of the mpu6050 are in turn connected to the SCL and SDA lines of the Pro-Mini. I am getting good data from the mpu6050, just not having any luck using the Uno as a slave device.

 

If anyone has some suggestions or advice about this, it would be greatly appreciated.

 

Thank you.

Link to comment
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

Loading...
 Share

×
×
  • Create New...