Using Wire with IBusBM

Projects Programming
1

I have a project I'm working on with a sketch that utilizes the Wire library and the IBusBM library. The problem is that it seems the two are incompatible? The sketch is attached.

Basically as long as I comment out Wire.endTransmission() and Wire.requestFrom() then everything works fine, except that the two motors being controlled run continuously because there is no input from the MPU6050.

If I uncomment them, then the RC Transmitter doesnt function. Any ideas and help greatly appreciated.



///////////////////////////////////////////////////////////////////////////////////////////////////////
// DO v2 iBus Sketch v1.1
// Sketch written by Reinhard Stockinger 2020/11
// Sketch is for the Printed Droid D-O Control PCB developed by Nitewing
// Latest skecth can always be found on www.printed-droid.com
///////////////////////////////////////////////////////////////////////////////////////////////////////
#include <Wire.h>
#include <IBusBM.h>
#include <Servo_Hardware_PWM.h>
#include "SoftwareSerial.h"
#include "DFRobotDFPlayerMini.h"


#define IBUS_ENABLED

//Define IBUS_Object for communication with RC via IBUS
#ifdef IBUS_ENABLED
  IBusBM ibusRc; // IBus object
#endif

HardwareSerial& ibusRcSerial = Serial1;
HardwareSerial& debugSerial = Serial;


//#define MAINBAR_CORRECTION
#define DFPLAYER_ENABLED
#define SERVOS_ENABLED


////////////////VARIABLE DEFINATION///////////////
//Cytron Motor Controller
int dir1pin =13; //Motor Direction pin (goes to DIR1)
int spe1pin =12; //Motor Speed pin (goes to PWM1)
int dir2pin =11; //Motor Direction pin (goes to DIR2)
int spe2pin =10; //Motor Speed pin (goes to PWM2)
int mspeed = 10; 
int turnspeed=50; 


int16_t Acc_rawX, Acc_rawY, Acc_rawZ,Gyr_rawX, Gyr_rawY, Gyr_rawZ;
float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];
float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180/3.141592654;
float PID, pwmLeft, pwmRight, error, previous_error;
float pid_p=20;
float pid_i=20;
float pid_d=0;
////////////////////////PID CONSTANST/////////////////////
float kp=25;
float ki=0;
float kd=0.8;
float desired_angle = -0.3;//////////////TARGET ANGLE///////////// measure the default angle and change this value

int pin1 = 3;  // This is input for RC (tank mixed) drive 1
int pin2 = 4;  // This is input for RC (tank mixed) drive 2
int duration1 = 1500; // Duration of the pulse from the RC
int duration2 = 1500; // Duration of the pulse from the RC
int motorspeed1 = 0;
int motordirection1 = HIGH;
int motorspeed2 = 0 ;
int motordirection2 = HIGH;

//error correction for mainbar!
int centreangle = -10; //offset, to get this figure, unrem the serial print angle (0)
int actualangle = 0; //actual angle for mapping main bar
int angleerror = 0; //this is the mainbar correction
int anglestrength =40; // this changes the severity of the adjustment (lower the number, the more the head corrects)

#ifdef SERVOS_ENABLED
  //Pins for the servos
  int pin_mainbarServo = 0;
  int pin_head1Servo = 1;
  int pin_head2Servo = 5;
  int pin_head3Servo = 6;
  
  Servo mainbarServo;
  Servo head1Servo;
  Servo head2Servo;
  Servo head3Servo;
#endif

//DF-Player defines
#ifdef DFPLAYER_ENABLED
  SoftwareSerial mySoftwareSerial (7, 8); // RX, TX
  DFRobotDFPlayerMini myDFPlayer;
  
  void printDetail(uint8_t type, int value);
  void handleDFPlayer();

  //Variables for reading the RC switches
  int readsw1;
  int readsw2;
  int readsw3;
  int readsw4;
  
  int dfpin1 = 14;
  int dfpin2 = 15;
  int dfpin3 = 16;
  int dfpin4 = 17;
  
  int currentsound = 2;
  int soundstate =1;
  int prevsoundstate =1;
#endif

void setup() 
{
  debugSerial.begin(74880);
  ibusRc.begin(ibusRcSerial);
  Wire.begin(); /////////////TO BEGIN I2C COMMUNICATIONS///////////////
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0);
  //Wire.endTransmission(true);
  

  ////////////////PIN MODE DEFINATIONS//////////////////////
  pinMode(dir1pin,OUTPUT);
  pinMode(spe1pin,OUTPUT);
  pinMode(dir2pin,OUTPUT);
  pinMode(spe2pin,OUTPUT);
  time = millis(); ///////////////STARTS COUNTING TIME IN MILLISECONDS/////////////

#ifdef IBUS_ENABLED
  //Serial1.begin(9600); //RX1 pin 19
  //ibusRc.begin(Serial1, IBUSBM_NOTIMER);
  ibusRc.loop();
  duration1 = ibusRc.readChannel(0); // Measure the input from rc drive 1
  duration2 = ibusRc.readChannel(1); // Measure the input from rc drive 1
#else
  duration1 = pulseIn(pin1, HIGH); //Measures the input from rc drive 1
  duration2 = pulseIn(pin2, HIGH); //Measures the input from rc drive 2
#endif


#ifdef SERVOS_ENABLED
  //Enable Servos
  mainbarServo.attach(pin_mainbarServo);
  mainbarServo.writeMicroseconds(1500); // set Mainbar to mid-point
  head1Servo.attach(pin_head1Servo);
  head1Servo.writeMicroseconds(1500); // center head 1 servo
  head2Servo.attach(pin_head2Servo);
  head2Servo.writeMicroseconds(1500); // center head 2 servo
  head3Servo.attach(pin_head3Servo);
  head3Servo.writeMicroseconds(1500); // center head 3 servo
#endif

#ifdef DFPLAYER_ENABLED
  mySoftwareSerial.begin(9600);
  if (!myDFPlayer.begin(mySoftwareSerial)) {  //Use softwareSerial to communicate with mp3.
    //Serial.println(F("Unable to begin:"));
    //Serial.println(F("1.Please recheck the connection!"));
    //Serial.println(F("2.Please insert the SD card!"));
    //while(true){
    //  delay(0); // Code to compatible with ESP8266 watch dog.
    //}
  }
  //Serial.println(F("DFPlayer Mini online."));
  myDFPlayer.volume(30);  //Set volume value. From 0 to 30
  delay (1000);
  myDFPlayer.play(1);  //Play the first mp3
#endif


  //this loops until it get a signal from RC, nothing will run
while ( duration1 >2200||duration1<800) {
  #ifdef IBUS_ENABLED
    ibusRc.loop();
    duration1 = ibusRc.readChannel(0);
    delay (300);
   #else    
    duration1 = pulseIn(pin1, HIGH); //Measures the input from rc drive 1  
  #endif
}
  
  //this loops until it get a signal from RC, nothing will run
while ( duration2 >2200||duration2<800) {                               
  #ifdef IBUS_ENABLED
      ibusRc.loop();
      duration2 = ibusRc.readChannel(1);
      delay (300);
  #else
      duration2 = pulseIn(pin2, HIGH); //Measures the input from rc drive 1  
  #endif
  }
  
}



// Read the number of a given channel and convert to the range provided.
// If the channel is off, return the default value
int readChannel(byte channelInput, int minLimit, int maxLimit, int defaultValue){
  uint16_t ch = ibusRc.readChannel(channelInput);
  if (ch < 100) return defaultValue;
  return map(ch, 1000, 2000, minLimit, maxLimit);
}



void loop() 
{

for (byte i = 0; i<9; i++){
    int value = readChannel(i, 800, 2200, 0);
    debugSerial.print("Ch");
    debugSerial.print(i + 1);
    debugSerial.print(": ");
    debugSerial.print(value);
    debugSerial.print(" ");
  }
debugSerial.println();

#ifdef IBUS_ENABLED
  ibusRc.loop();
  duration1 = ibusRc.readChannel(0); // Measure the input from rc drive 1
  duration2 = ibusRc.readChannel(1); // Measure the input from rc drive 1
#else  
  duration1 = pulseIn(pin1, HIGH); //Measures the input from rc drive 1
  duration2 = pulseIn(pin2, HIGH); //Measures the input from rc drive 2
#endif  


  //Serial.print (duration1);
  //Serial.print (" ");
  //Serial.print (duration2);
  //Serial.print (" \n");

  motorspeed1 = map (duration1,1000,2000,-255,255); //Maps the duration to the motorspeed from the stick
  motorspeed2 = map (duration2,1000,2000,-255,255); //Maps the duration to the motorspeed from the stick
  //Serial.print (motorspeed1);
  //Serial.print (" ");
  //Serial.print (motorspeed2);
  //Serial.print (" ");

#ifdef SERVOS_ENABLED
  #ifdef MAINBAR_CORRECTION
    mainbarServo.writeMicroseconds((ibusRc.readChannel(2)+angleerror));
  #else
    mainbarServo.writeMicroseconds(ibusRc.readChannel(2));
  #endif  
  
  head1Servo.writeMicroseconds(ibusRc.readChannel(3));
  head2Servo.writeMicroseconds(ibusRc.readChannel(4));
  head3Servo.writeMicroseconds(ibusRc.readChannel(5));
#endif

#ifdef DFPLAYER_ENABLED
  handleDFPlayer();
#endif
  
  /////////////////////////WARNING//////////////////////
  // * DO NOT USE ANY DELAYS INSIDE THE LOOP OTHERWISE THE BOT WON'T BE 
  // * ABLE TO CORRECT THE BALANCE FAST ENOUGH
  // * ALSO, DONT USE ANY SERIAL PRINTS. BASICALLY DONT SLOW DOWN THE LOOP SPEED.
  
  timePrev = time;  
  time = millis();  
  elapsedTime = (time - timePrev) / 1000; 
  Wire.beginTransmission(0x68);
  Wire.write(0x3B); 
  //Wire.endTransmission(false);
  //Wire.requestFrom(0x68,6,true);

  
  ////////////////////PULLING RAW ACCELEROMETER DATA FROM IMU///////////////// 
  Acc_rawX=Wire.read()<<8|Wire.read(); 
  Acc_rawY=Wire.read()<<8|Wire.read();
  Acc_rawZ=Wire.read()<<8|Wire.read(); 
  /////////////////////CONVERTING RAW DATA TO ANGLES/////////////////////
  Acceleration_angle[0] = atan((Acc_rawY/16384.0)/sqrt(pow((Acc_rawX/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
  Acceleration_angle[1] = atan(-1*(Acc_rawX/16384.0)/sqrt(pow((Acc_rawY/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
  Wire.beginTransmission(0x68);
  Wire.write(0x43); 
  //Wire.endTransmission(false);
  //Wire.requestFrom(0x68,4,true); 

  
  //////////////////PULLING RAW GYRO DATA FROM IMU/////////////////////////
  Gyr_rawX=Wire.read()<<8|Wire.read(); 
  Gyr_rawY=Wire.read()<<8|Wire.read(); 
  //Serial.print("rawx ");
  //Serial.print(Gyr_rawX);
  //Serial.print("rawy ");
  //Serial.print(Gyr_rawY);
  //Serial.print("\n");
  ////////////////////CONVERTING RAW DATA TO ANGLES///////////////////////
  Gyro_angle[0] = Gyr_rawX/131.0; 
  Gyro_angle[1] = Gyr_rawY/131.0;
  //////////////////////////////COMBINING BOTH ANGLES USING COMPLIMENTARY FILTER////////////////////////
  Total_angle[0] = 0.98 *(Total_angle[0] + Gyro_angle[0]*elapsedTime) + 0.02*Acceleration_angle[0];
  Total_angle[1] = 0.98 *(Total_angle[1] + Gyro_angle[1]*elapsedTime) + 0.02*Acceleration_angle[1];
  
  ////TOTAL_ANGLE[0] IS THE PITCH ANGLE WHICH WE NEED////////////
  error = Total_angle[0] - desired_angle; 
  
  /////////////////ERROR CALCULATION////////////////////
  ///////////////////////PROPORTIONAL ERROR//////////////
  pid_p = kp*error;
  ///////////////////////INTERGRAL ERROR/////////////////
  pid_i = pid_i+(ki*error);  
  ///////////////////////DIFFERENTIAL ERROR//////////////
  pid_d = kd*((error - previous_error)/elapsedTime);
  ///////////////////////TOTAL PID VALUE/////////////////
  PID = pid_p + pid_d;
  ///////////////////////UPDATING THE ERROR VALUE////////
  previous_error = error;
  //Serial.println(PID);                     //////////UNCOMMENT FOR DDEBUGGING//////////////
  //delay(60);                               //////////UNCOMMENT FOR DDEBUGGING//////////////
  //Serial.println(Total_angle[0]);          //////////UNCOMMENT FOR DDEBUGGING//////////////
  /////////////////CONVERTING PID VALUES TO ABSOLUTE VALUES//////////////////////////////////
  mspeed = abs(PID);

  actualangle=Total_angle[0] - centreangle;
  angleerror=map(actualangle,anglestrength,-anglestrength,1000,2000)-1500;
  //Serial.println(angleerror);

  mspeed=map(mspeed,0,2100,0,700); // ** The last number  mspeed=map(mspeed,0,2100,0,700) - 700, can be changed to increase or decrease the "harshness" of the speed compensation when balancing. 
    
  //Serial.println(mspeed);                  //////////UNCOMMENT FOR DDEBUGGING//////////////
  ///////////////SELF EXPLANATORY///////////////
   
  if(Total_angle[0]<0+desired_angle)
  {
    mspeed=-mspeed;
  }
  if(Total_angle[0]>0+desired_angle)
  {
    mspeed = mspeed;
  }
    
  motorspeed1=motorspeed1+mspeed; //This add the RC drive to the correction drive, motorspeed is the rc, mspeed from the IMU
  motorspeed2=motorspeed2-mspeed; //This add the RC drive to the correction drive, motorspeed is the rc, mspeed from the IMU
  //Serial.println (mspeed);
  //Serial.print (" ");
  //Serial.print (mspeed);
  //Serial.print (" ");

  if (motorspeed1<0) {
    motordirection1 = LOW;
    motorspeed1=-motorspeed1;
  }
  else if (motorspeed1>0) {
    motordirection1 = HIGH;  
  }

  if (motorspeed2<0) {
    motordirection2 = LOW;
    motorspeed2=-motorspeed2;
  }
  else if (motorspeed2>0) {
    motordirection2 = HIGH;  
  }

  if (motorspeed1 >254){
    motorspeed1=255;
  }

  if (motorspeed2 >254){
    motorspeed2=255;
  }

  //Serial.print (motorspeed1);
  //Serial.print (" ");
  //Serial.print (motorspeed2);
  //Serial.println (" ");

  digitalWrite(dir1pin,motordirection1);
  analogWrite(spe1pin,motorspeed1); //increase the speed of the motor from 0 to 255
  digitalWrite(dir2pin,motordirection2);
  analogWrite(spe2pin,motorspeed2); //increase the speed of the motor from 0 to 255
}

/************************************************************************/


#ifdef DFPLAYER_ENABLED
void handleDFPlayer()
{
#ifdef IBUS_ENABLED
  ibusRc.loop();
  readsw1 = ibusRc.readChannel(6);
  readsw2 = ibusRc.readChannel(7);
  readsw3 = ibusRc.readChannel(8);
  readsw4 = ibusRc.readChannel(9);
#else
  readsw1 = pulseIn (dfpin1, HIGH);
  readsw2 = pulseIn (dfpin2, HIGH);
  readsw3 = pulseIn (dfpin3,HIGH);
  readsw4 = pulseIn (dfpin4,HIGH);
#endif

  if (readsw1< 1500){
    soundstate=1;
  }
  if (readsw1> 1500){
    soundstate=2;
  }
  if (soundstate != prevsoundstate){
    prevsoundstate = soundstate;
    myDFPlayer.play(currentsound);  //Play the first mp3
  }
  if (readsw2 > 1500){
    currentsound = random (3,6);
  }
  if (readsw2 < 1500){
    currentsound = 2;
  }
  if (readsw3 > 1300){
    currentsound = random (6,10);
  }
  if (readsw3 > 1800){
    currentsound = random (10,15);
  }
  if (readsw4 > 1500){
    currentsound = random (15,21);
  }

  if (myDFPlayer.available()) {
    printDetail(myDFPlayer.readType(), myDFPlayer.read()); //Print the detail message from DFPlayer to handle different errors and states.
  }

}

/************************************************************************/

void printDetail(uint8_t type, int value){
  switch (type) {
    case TimeOut:
      //Serial.println(F("Time Out!"));
      break;
    case WrongStack:
      //Serial.println(F("Stack Wrong!"));
      break;
    case DFPlayerCardInserted:
      //Serial.println(F("Card Inserted!"));
      break;
    case DFPlayerCardRemoved:
      //Serial.println(F("Card Removed!"));
      break;
    case DFPlayerCardOnline:
      //Serial.println(F("Card Online!"));
      break;
    case DFPlayerUSBInserted:
      //Serial.println("USB Inserted!");
      break;
    case DFPlayerUSBRemoved:
      //Serial.println("USB Removed!");
      break;
    case DFPlayerPlayFinished:
      //Serial.print(F("Number:"));
      //Serial.print(value);
      //Serial.println(F(" Play Finished!"));
      break;
    case DFPlayerError:
      //Serial.print(F("DFPlayerError:"));
      switch (value) {
        case Busy:
          //Serial.println(F("Card not found"));
          break;
        case Sleeping:
         // Serial.println(F("Sleeping"));
          break;
        case SerialWrongStack:
         //Serial.println(F("Get Wrong Stack"));
          break;
        case CheckSumNotMatch:
          //Serial.println(F("Check Sum Not Match"));
          break;
        case FileIndexOut:
          //Serial.println(F("File Index Out of Bound"));
          break;
        case FileMismatch:
         // Serial.println(F("Cannot Find File"));
          break;
        case Advertise:
          //Serial.println(F("In Advertise"));
          break;
        default:
          break;
      }
      break;
    default:
      break;
  }
}

#endif

dov2_printed_droid_rc_ibus_v1.2.ino (14.7 KB)

2

Those are the only two functions that actually use the hardware I2C bus. That means that if you use the I2C bus then the sketch does something wrong.

Does the rest of the sketch still work ? Is it only the RC Transmitter that does no longer work ? Are you sure that the rest still works ? A problem with wiring of the I2C bus can halt a sketch.

Do you use an Arduino Mega board ?

I assume that you installed the IBusBM from the Library Manager. That is this library: https://github.com/bmellink/IBusBM
If you also installed the Servo_Hardware_PWM library from the Library Manager, then it is this one: https://github.com/dadul96/Arduino-Servo-Hardware-PWM-Library

Can you replace the SoftwareSerial port with a hardware serial port ?
Why do you use 74880 baud for the Serial Monitor ? Normally 115200 or 9600 is used.

There are many interrupts in the sketch, and there are also delays in the sketch. Those many interrupts could get in the way of each other. If you don't use SoftwareSerial anymore, that will be a big relief.
I don't see a problem between the IBusBM library and the Wire library.

3

OK. I played around with it a bit and the only way anything works is by uncommenting the 3 Wire.endTransmission() lines and the 2 Wire.requestFrom() lines.

When I do that, everything works as expected (sound triggering, servo and motor control, etc). The only thing is that the motors run continuously without stopping. Using the RC transmitter slows them down, but still run continuously.

It seems that those lines are needed for communication with the MPU6050.

Any other way to do this?

4

There are a lot of detailed questions in reply #2 that you didn't answer. Even a simple one like what board you are using. People will hesitate to offer further advice if questions are left untouched.

5

I'm so sorry.

Does the rest of the sketch still work ? Is it only the RC Transmitter that does no longer work ? >Are you sure that the rest still works ? A problem with wiring of the I2C bus can halt a sketch.
Do you use an Arduino Mega board ?

When those lines are commented out, the rest of the sketch works fine. When left in, nothing works. Using an Arduino Mega 2560 board with custom shield found here:

DO Control and Power Boards

IbusBM and Servo_Hardward_PWM are installed from the library manager.

Can you replace the SoftwareSerial port with a hardware serial port ?
Why do you use 74880 baud for the Serial Monitor ? Normally 115200 or 9600 is used.

I'll try replacing the SoftwareSerial with hardware. Baud rate was an artifact of the sketch source.

6

Also with the custom board I may not be able to replace the SoftwareSerial with Hardware since it is defining pins 8,9 as software serial as opposed to the normal serial pins. Am I correct on that?

7

They don't provide a schematic ? That's not good. They build upon open-source Arduino but then keep their own shield a secret.

The hardware serial ports of the Arduino Mega are at fixed pins. You can see the pins in the table: https://www.arduino.cc/reference/en/language/functions/communication/serial/

When there is something wrong with the I2C bus, then the sketch halts. For example when SDA is shortcut to GND or the SCL is shortcut to GND or SCL is shortcut to SDA or the MPU-6050 module is not powered.
Can you run a I2C Scanner sketch ? If that sketch does not run, then you have to search one of those hardware problems until the I2C Scanner works and it can see the MPU-6050.

8

It doesn't stop you cutting tracks and adding jumper wires.

Tom.... :smiley: :+1: :coffee: :australia:

9

Can you run a I2C Scanner sketch ?

I tried running this sketch:

// --------------------------------------
// i2c_scanner
//
// Version 1
//    This program (or code that looks like it)
//    can be found in many places.
//    For example on the Arduino.cc forum.
//    The original author is not know.
// Version 2, Juni 2012, Using Arduino 1.0.1
//     Adapted to be as simple as possible by Arduino.cc user Krodal
// Version 3, Feb 26  2013
//    V3 by louarnold
// Version 4, March 3, 2013, Using Arduino 1.0.3
//    by Arduino.cc user Krodal.
//    Changes by louarnold removed.
//    Scanning addresses changed from 0...127 to 1...119,
//    according to the i2c scanner by Nick Gammon
//    https://www.gammon.com.au/forum/?id=10896
// Version 5, March 28, 2013
//    As version 4, but address scans now to 127.
//    A sensor seems to use address 120.
// Version 6, November 27, 2015.
//    Added waiting for the Leonardo serial communication.
// 
//
// This sketch tests the standard 7-bit addresses
// Devices with higher bit address might not be seen properly.
//

#include <Wire.h>


void setup()
{
  Wire.begin();

  Serial.begin(9600);
  while (!Serial);             // Leonardo: wait for serial monitor
  Serial.println("\nI2C Scanner");
}


void loop()
{
  byte error, address;
  int nDevices;

  Serial.println("Scanning...");

  nDevices = 0;
  for(address = 1; address < 127; address++ ) 
  {

    //Track where we are at in loop
    Serial.print("Testing Address : ");
    Serial.println(address);
    
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    Wire.beginTransmission(address);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknown error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");

  delay(500);           // wait 0.5 seconds for next scan
}

and my output is

I2C Scanner
Scanning...
Testing Address : 1

And seems to be running in an infinite loop.

Also here are the schematics he sent me:
306014484_1156238738291103_2216801368524329638_n

10

Your I2C Scanner sketch halts in Wire.endTransmission(), that happens with a shortcut on the I2C bus.
You have to find the shortcut or the missing power to the sensor. You don't need to try something else.

11

Starting to think I have a bad MPU6050.

Using JUST the Arduino Mega 2560 and the MPU6050, wired as follows:

MPU - MEGA
5v - 5v
GND - GND
SCL. - SCL.
SDA - SDA
INT. - Digital Pin 2

Running the MPU6050_gyro_simple sketch:

/*
    MPU6050 Triple Axis Gyroscope & Accelerometer. Simple Gyroscope Example.
    Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html
    GIT: https://github.com/jarzebski/Arduino-MPU6050
    Web: http://www.jarzebski.pl
    (c) 2014 by Korneliusz Jarzebski
*/

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

void setup() 
{
  Serial.begin(115200);

  // Initialize MPU6050
  Serial.println("Initialize MPU6050");
  while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  {
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    delay(500);
  }
  
  // If you want, you can set gyroscope offsets
  // mpu.setGyroOffsetX(155);
  // mpu.setGyroOffsetY(15);
  // mpu.setGyroOffsetZ(15);
  
  // Calibrate gyroscope. The calibration must be at rest.
  // If you don't want calibrate, comment this line.
  mpu.calibrateGyro();

  // Set threshold sensivty. Default 3.
  // If you don't want use threshold, comment this line or set 0.
  mpu.setThreshold(3);
  
  // Check settings
  checkSettings();
}

void checkSettings()
{
  Serial.println();
  
  Serial.print(" * Sleep Mode:        ");
  Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled");
  
  Serial.print(" * Clock Source:      ");
  switch(mpu.getClockSource())
  {
    case MPU6050_CLOCK_KEEP_RESET:     Serial.println("Stops the clock and keeps the timing generator in reset"); break;
    case MPU6050_CLOCK_EXTERNAL_19MHZ: Serial.println("PLL with external 19.2MHz reference"); break;
    case MPU6050_CLOCK_EXTERNAL_32KHZ: Serial.println("PLL with external 32.768kHz reference"); break;
    case MPU6050_CLOCK_PLL_ZGYRO:      Serial.println("PLL with Z axis gyroscope reference"); break;
    case MPU6050_CLOCK_PLL_YGYRO:      Serial.println("PLL with Y axis gyroscope reference"); break;
    case MPU6050_CLOCK_PLL_XGYRO:      Serial.println("PLL with X axis gyroscope reference"); break;
    case MPU6050_CLOCK_INTERNAL_8MHZ:  Serial.println("Internal 8MHz oscillator"); break;
  }
  
  Serial.print(" * Gyroscope:         ");
  switch(mpu.getScale())
  {
    case MPU6050_SCALE_2000DPS:        Serial.println("2000 dps"); break;
    case MPU6050_SCALE_1000DPS:        Serial.println("1000 dps"); break;
    case MPU6050_SCALE_500DPS:         Serial.println("500 dps"); break;
    case MPU6050_SCALE_250DPS:         Serial.println("250 dps"); break;
  } 
  
  Serial.print(" * Gyroscope offsets: ");
  Serial.print(mpu.getGyroOffsetX());
  Serial.print(" / ");
  Serial.print(mpu.getGyroOffsetY());
  Serial.print(" / ");
  Serial.println(mpu.getGyroOffsetZ());
  
  Serial.println();
}

void loop()
{
  Vector rawGyro = mpu.readRawGyro();
  Vector normGyro = mpu.readNormalizeGyro();

  Serial.print(" Xraw = ");
  Serial.print(rawGyro.XAxis);
  Serial.print(" Yraw = ");
  Serial.print(rawGyro.YAxis);
  Serial.print(" Zraw = ");
  Serial.println(rawGyro.ZAxis);

  Serial.print(" Xnorm = ");
  Serial.print(normGyro.XAxis);
  Serial.print(" Ynorm = ");
  Serial.print(normGyro.YAxis);
  Serial.print(" Znorm = ");
  Serial.println(normGyro.ZAxis);
  
  delay(10);
}

And gets stuck at Initialize MPU6050.

12

Same problem, a shortcut on the I2C bus.
Was this without the shield ? Then the problem is on the Mega board or on the sensor module. Do you have another sensor module ?

For now, you only need the I2C Scanner sketch.

13

Testing Address : 104
I2C device found at address 0x68 !

Turns out it was a bad MPU6050 unit. Sheesh! What a headache!

Going to try wiring everything back up and testing it.

14

Bad MPU6050 Unit fixed everything! Here I thought it was something in the sketch.

Thank you everyone for helping!!!

15

Good :smiley:

If you also can replace the SoftwareSerial with a hardware Serial port, then the interrupts will run better.

16

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.