MPU-6050 Code is Disabling My Motor Speed Control

Using Arduino Sensors
#1

Hello

I’m using an ino file developed for the MPU-6050 by Ahmet Burkay Kirk. I have included it below.
I like this code because I don’t have to write my own code to take raw IMU data and convert into angles. I’m trying to build two-wheeled self-balancing robot and the angles, as defined in this algorithm work perfectly so I can measure the angles about the x axis.

The problem I’m having is that when I copy and paste the code into my existing ino file, I lose control over my DC motor speed. I still retain control over the direction but the speed caps off and does not max out when commanded. Currently, I’m using a potentiometer to simulate the robot tilt angle. Then the ino file takes the simulated angle and determines DC motor direction and speed.

I copied Ahmet’s code and pasted a few lines at a time and was fine until I added the following blocks:
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);

So, I’m sure has to do with the fact that the transmission of the “wire” data is preventing the motor speed control. Below is the original code from Ahmet. I’ve also attached my file which attempts to incorporate the MPU algorithm with my motor control algorithm. Keep in mind that the attached ino file is not trying to use the IMU data yet. It’s actually using potentiometer data from A0 to simulate the tilt angle. But including the MPU-6050 algorithm into it disrupts my DC motor speed control. Any help will be helpful.

//Written by Ahmet Burkay KIRNIK
//TR_CapaFenLisesi
//Measure Angle with a MPU-6050(GY-521)

#include<Wire.h>

const int MPU_addr=0x68;
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

int minVal=265;
int maxVal=402;

double x;
double y;
double z;

void setup(){
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read();
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);

x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);

Serial.print("AngleX= ");
Serial.println(x);

Serial.print("AngleY= ");
Serial.println(y);

Serial.print(“AngleZ= “);
Serial.println(z);
Serial.println(”-----------------------------------------”);
delay(400);
}

Thank you.
Rick

sketch_DCStepperMotorControl_2Apr2017.ino (6.36 KB)

#2

the thought of delay(400); is bothering me. This blocks the arduino from doing anything for almost half a second!!!
consider

  int t = 400; // delay in seconds
  static unsigned long _ETimer;
  if ( millis() - _ETimer >= (t)) {
    _ETimer += (t);
    // your code here for exact timing but windup occurs if other delays are present
  }

  static unsigned long _ATimer;
  if ((millis() - _ATimer) >= (t)) {
    _ATimer = millis();
    // your code here for after timing this is not exact but no windup can occur (great spam timer)
  }

Example:

  int t = 400;  
  static unsigned long _ETimer;
  if ( millis() - _ETimer >= (t)) {
    _ETimer += (t);
      Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);
  AcX=Wire.read()<<8|Wire.read();
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();
    int xAng = map(AcX,minVal,maxVal,-90,90);
    int yAng = map(AcY,minVal,maxVal,-90,90);
    int zAng = map(AcZ,minVal,maxVal,-90,90);

       x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
       y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
       z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);

     Serial.print("AngleX= ");
     Serial.println(x);

     Serial.print("AngleY= ");
     Serial.println(y);

     Serial.print("AngleZ= ");
     Serial.println(z);
     Serial.println("-----------------------------------------");
    // delay(400);
  }
#3

Thank you HC. I don't think it's the .4 second delay. Even after commenting out the 400 ms delay, I still lose motor speed control. Without the Wire transmit functions, my speed control works fine. I attempted your recommended fix but no luck.

#4