Run MPU-6050 and Roboclaw together

[Arman24]

Hello ,
 I experiencing some Problems with my MPU6050. I want to construct a self balancing robot on two wheels. Therefore i need the IMU and a Motorshield (RoboClaw 2x15A Motor Controller V4). If i run the IMU alone all works fine. Its the same with the Motorshield. But if i try to run the IMU and the Motorshield together i cannot initialize the IMU anymore. Both devices need serial communication. It would be really nice if someone could help me.

The Motorshield is connected to the rx and tx (0,1) pin of my Arduino Uno.
The Interrupt Pin of the IMU to digital pin 2, SCL to analog 5 and SDA to analog 4

Best Regards
Arman Shakupbekow

 

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "BMSerial.h"
#include "RoboClaw.h"

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

#define address 0x80
#define LED_PIN 13
#define runEvery(t) for (static long _lasttime;\
                         (uint16_t)((uint16_t)millis() - _lasttime) >= (t);\
                         _lasttime += (t))

MPU6050 mpu;
RoboClaw roboclaw (0,1,10000,true);
Quaternion q;
VectorFloat gravity;
uint8_t devStatus;
uint8_t mpuIntStatus;
uint8_t fifoBuffer[64];
uint16_t packetSize;
uint16_t fifoCount;
bool dmpReady = false;
bool blinkState = false;
volatile bool mpuInterrupt = false;
int tx = 1;
int rx = 0;
float ypr[3];
double intendedAngle;
double setpoint, input, output;
int posOutMax = 127;
int negOutMin = -24;
int posOutMin = 24;
int negOutMax = -127;
float lastError = 0;
double ITerm =0;
double kp = 1.0;         
double ki = 0.0;           
double kd = 1.0;  

void initIMU();
void dmpDataReady();
void initMotors();
void setSetpoint(double);
float getAngle();
void initPID();
double computePID(float);
void motorcontrol(double);

void setup()
{
  initIMU();
  initMotors();
  setSetpoint(0.0);
  initPID();
}

void loop()
{
 
    float Angle = getAngle();
    double PIDOutput = computePID(Angle);
    motorcontrol(PIDOutput);
    //Serial.println(PIDOutput);
 
}

void initIMU()
{
  #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(38400);
  while (!Serial);
  mpu.initialize();
  devStatus = mpu.dmpInitialize();
  mpu.setXGyroOffset(50); // 220
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788);
  if (devStatus == 0)
  {
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(1, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  }
  else   
  {
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

  pinMode(LED_PIN, OUTPUT);
}

void dmpDataReady()
{
  mpuInterrupt = true;
}

float getAngle()
{
  while (!mpuInterrupt && fifoCount < packetSize)
  {
  }
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();
  if ((mpuIntStatus & 0x10) || fifoCount == 1024)
  {
    mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));
  }
  else if (mpuIntStatus & 0x02)
  {
    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);
    //Serial.print("ypr\t");
    //Serial.print(ypr[0] * 360/M_PI);
    //Serial.print("\t");
    //Serial.print(ypr[1] * 360/M_PI);
    Serial.print("Pitch: ");
    Serial.println(ypr[1] * 360/M_PI);
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
    return (ypr[1] *   360/M_PI);
    
  }
}

void initMotors()
{
  roboclaw.begin(38400);
}

void setSetpoint(double setp)
{
  intendedAngle = setp;
}
void initPID()
{
  setpoint = intendedAngle;
}

double computePID(float pitch)
{
  double error = setpoint - pitch;
  ITerm+= (ki * error);
  if(ITerm > posOutMax) ITerm= posOutMax;
  else if(ITerm < negOutMax) ITerm= negOutMax;
  double dError = (error - lastError);
  double output = kp * error + ITerm + kd * dError;
      
  if(output > 0)
  {
    output = posOutMin + output;
  }
  else if(output < 0)
  {
    output = negOutMin + output;
  }   
 
  if(output > posOutMax)
 {
   output = posOutMax;
 }
 else if ( output < negOutMax )
 {
   output = negOutMax;
 }
 
  lastError = error;
  //Serial.print("error: ");
  //Serial.println(error);
  return output;
}

void motorcontrol(double out)
{
  if (out > 0)
  {              
    byte vel = abs(out);   
    if (vel<0)
      vel=0;
    if (vel > 127)
      vel=127;
    //roboclaw.BackwardM1(address,vel);
    //roboclaw.BackwardM2(address,vel);    
  }
  else
  {                     
    byte vel = abs(out);   
    if (vel<0)
      vel=0;
    if (vel > 127)
      vel=127;
    //roboclaw.ForwardM1(address,vel);
    //roboclaw.ForwardM2(address,vel);    
  }  
}