MPU6050 stops sending data

Projects Programming
1

I'm using Arduino Uno with MPU 6050 and a quadrature encoder.
Attached is the .ino file I have been using.
This code is supposed to go a distance in a straight line(kept straight by PID using Gyro readings) according to encoder readings, then take a U-Turn once a certain encoder reading has been reached. It stops for a second after taking the U-Turn and then starts straight line motion again.
The problem I'm facing is that the gyroscope readings stop randomly while taking readings during the turn. Due to this, it keeps rotating about that point without stopping. The serial monitor just stops displaying the reading, and the robot keeps rotating without stopping. This does not happen every time though, and it is very random as to at what reading it stops working. Please help.

mode1a.ino (7.34 KB)

2

Code posted properly. Please use code tags.

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

MPU6050 mpu;
Encoder myEnc(11, 12);

long newP=0, newPosition=0, newPo=0, oldPo=0;//Encoder values

unsigned long currentTime, previousTime, elapsedTime;
double err, lastError, output, setPoint, cumError, rateError, kp=40, ki=0, kd=0, yaw=0;
// Timers
unsigned long timer = 0,timera=0;
float timeStep = 0.01;

const int motor11=6;//LEFT MOTOR FW
const int motor12=7;//LEFT MOTOR BK
const int motor1pwm=3;
const int motor21=8;//RIGHT MOTOR FW
const int motor22=9;//RIGHT MOTOR BK
const int motor2pwm=5;

int thres1=120;//PWM Values
int thres2=120;

int flag1=0, flag2=0, flag3=0, flag4=1, flag5=0;//flag1 is for set point, flag2 is for slow start, flag3, flag4 for changing x, flag5 for changing y

int x=0, y=0, save=0;//x, y store coordinate. Store is to prevent counting repeated coordinates while calculating coordinates

void setup() 
{
    Serial.begin(115200);
    // Initialize MPU6050  
    while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
    {
      Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
      delay(500);
    }
    mpu.calibrateGyro();
    mpu.setThreshold(1);
  
    pinMode(motor11, OUTPUT);
    pinMode(motor12, OUTPUT);
    pinMode(motor21, OUTPUT);
    pinMode(motor22, OUTPUT);
    pinMode(motor1pwm, OUTPUT);
    pinMode(motor2pwm, OUTPUT);
}

void loop()
{
    newP = myEnc.read()/4;
    newPo=newP-newPosition;
    Serial.print("Encoder ");
    Serial.println(newPo);

    Vector norm = mpu.readNormalizeGyro();
    yaw = yaw + norm.ZAxis * timeStep;
             
    if(abs(newPo)<=600) //Straight line motion
    {
        if(abs(newPo)%60==0)
        {
          if(flag3==0)//Increase x- coordinate
          {
            if(save<abs(newPo))
            {
              if(abs(newPo)>abs(oldPo))
              {
                x=x+1;
                Serial.print("X= ");
                Serial.println(x);
                Serial.print("Y= ");
                Serial.println(y);
                if(x==10)
                {
                  flag3=-1;
                }
                save=abs(newPo);
              }
              else if(abs(newPo)<=abs(oldPo))
              {
                save=abs(oldPo);
              }
           }
         }
        
          else if(flag4==0)//Decrease x-coordinate
          {
            if(save<abs(newPo))
            {
              if(abs(newPo)>abs(oldPo))
              {
                x=x-1;
                Serial.print("X= ");
                Serial.println(x);
                Serial.print("Y= ");
                Serial.println(y);
                if(x==0)
                {
                  flag4=1;
                }
                save=abs(newPo);
              }
              else if(abs(newPo)<=abs(oldPo))
              {
                save=abs(oldPo);
              }
            }
          }
        }
      
      if(flag1==0)//Set Point
      {
        setPoint=yaw;
        Serial.print("Set Point= ");
        Serial.println(setPoint);
        flag1=-1;
      }
  
      if(flag2==0)//slow start
      {
          digitalWrite(motor21, HIGH);
          digitalWrite(motor22, LOW);
          analogWrite(motor2pwm, 75);
          digitalWrite(motor11, HIGH);
          digitalWrite(motor12, LOW);
          analogWrite(motor1pwm, 75);
          delay(500);
          flag2=-1;
      }
      
      timer = millis();
      currentTime = millis();
      elapsedTime = currentTime - previousTime;             
      err = setPoint - yaw; 
      cumError = cumError+(err * elapsedTime); 
      rateError = (err - lastError)/elapsedTime;
      output = kp*err+ki*cumError+kd*rateError;
      lastError = err;  
      previousTime = currentTime;  
  
      if(err>0.001)
      {
        Serial.print("1. YAW= ");
        Serial.println(yaw);
        digitalWrite(motor11, HIGH);
        digitalWrite(motor12, LOW);
        analogWrite(motor1pwm, thres2);
        digitalWrite(motor21, HIGH);
        digitalWrite(motor22, LOW);
  
        if(thres1+output<255)
        {
          analogWrite(motor2pwm, thres1+output);
        }
  
        else
        {
          analogWrite(motor2pwm, 255);
        }
      }
  
      else if(err<-0.001)
      {
        Serial.print("2. YAW= ");
        Serial.println(yaw);
        digitalWrite(motor11, HIGH);
        digitalWrite(motor12, LOW);
  
        if(thres2-output<255)
        {
          analogWrite(motor1pwm, thres2-output);
        }
  
        else
        {
          analogWrite(motor1pwm, 255);
        }
        digitalWrite(motor21, HIGH);
        digitalWrite(motor22, LOW);
        analogWrite(motor2pwm, thres1);  
      }
  
      else if((err>-0.001)&&(err<0.001))
      {
        Serial.print("3. YAW= ");
        Serial.println(yaw);
        digitalWrite(motor11, HIGH);
        digitalWrite(motor12, LOW);
        analogWrite(motor1pwm, thres2);   
        digitalWrite(motor21, HIGH);
        digitalWrite(motor22, LOW);
        analogWrite(motor2pwm, thres1);     
      } 

      oldPo=newPo;
    
      flag5=0;
      delay((timeStep*200) - (millis() - timer));
  }
   
    else if(abs(newPo)>600)
    {            
      digitalWrite(motor21, HIGH);
      digitalWrite(motor22, LOW);
      analogWrite(motor2pwm, 90);
      digitalWrite(motor11, HIGH);
      digitalWrite(motor12, LOW);
      analogWrite(motor1pwm, 90);
      
      delay(300);
      
      if(flag5==0)
      {
        y=y+1;
        flag5=-1;
      }
      
      if(y%2==0)
      {       
      
        while((abs(setPoint)-abs(yaw)<540)&&(yaw<0.2))
        {
          //timera=millis();
          Vector norm = mpu.readNormalizeGyro();
          yaw = yaw + norm.ZAxis * timeStep;
          Serial.print("Turning Yaw= ");
          Serial.println(yaw);
          digitalWrite(motor11, LOW);
          digitalWrite(motor12, LOW);
          analogWrite(motor1pwm, 120);   
          digitalWrite(motor21, HIGH);
          digitalWrite(motor22, LOW);
          analogWrite(motor2pwm, 120);
          //delay((timeStep*2000) - (millis() - timera));
        }    
        flag3=0;
      }  
       
      else if((y+1)%2==0)
      {    
          while(abs(yaw)-abs(setPoint)<540)
          {
            //timera=millis();
            Vector norm = mpu.readNormalizeGyro();
            yaw = yaw + norm.ZAxis * timeStep;
            Serial.print("Turning Yaw= ");
            Serial.println(yaw);
            digitalWrite(motor11, HIGH);
            digitalWrite(motor12, LOW);
            analogWrite(motor1pwm, 120);   
            digitalWrite(motor21, LOW);
            digitalWrite(motor22, LOW);
            analogWrite(motor2pwm, 120);
            //delay((timeStep*2000) - (millis() - timera));
          }
          flag4=0;
          if(y==5)
          {
            while(true)
           {
              digitalWrite(motor11, LOW);
              digitalWrite(motor12, LOW);
              analogWrite(motor1pwm, 70);   
              digitalWrite(motor21, LOW);
              digitalWrite(motor22, LOW);
              analogWrite(motor2pwm, 30);          
            }
          }
      }
      
      digitalWrite(motor11, LOW);
      digitalWrite(motor12, LOW);
      analogWrite(motor1pwm, thres2);   
      digitalWrite(motor21, LOW);
      digitalWrite(motor22, LOW);
      analogWrite(motor2pwm, thres1);
  
      flag1=0;
      flag2=0;
     
      newPosition = myEnc.read()/4;
    
      save=0;
      oldPo=0;
      
      delay(2000);
    }
}
3

Okay. Are you aware of what the problem could be?

4

No. Consider adding comments to the code, so that readers might have some idea what various parts are imagined to do.

Please provide evidence for this assertion:

the gyroscope readings stop randomly while taking readings during the turn

General comment: gyros drift uncontrollably. A properly calibrated magnetometer is a much better choice for determining yaw angles.