Straight line with MPU6050

I wrote this few line of code in order to allowing my little robot with 2 DC motor and MPU6050 to follow a straight line when moving forward.
But something is not correct…
with the func GetMPUValue() i’m getting the yaw of the robot and than if the robot steering to the left or to the right i’m try to adjust the wheel speed according to the direction in order to bring it back to a yaw between -0.8 and 0.8.
but… looks like is to slow to perform this correction left and right
thanks… for helping me…
dm1886

#include <Arduino.h>
#include <NewPing.h>
#include <Servo.h>
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  #include "Wire.h"
#endif
#include "MPU6050_6Axis_MotionApps20.h"
#include <MPU6050.h>

// motore 1
const int motor1_speedPin = 10; 
const int ava1 = 9;
const int ind1 = 8;
int LeftWheelSpeed = 60;
//motore 2
const int motor2_speedPin = 5; 
const int ava2 = 6;
const int ind2 = 7;
int RightWheelSpeed = 60;

MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false; 
uint8_t mpuIntStatus; 
uint8_t devStatus; 
uint16_t packetSize; 
uint16_t fifoCount; 
uint8_t fifoBuffer[64];
int mpuValue;
boolean mpuValueNegative = false;
// orientation/motion vars
Quaternion q;
VectorFloat gravity; 
float ypr[3]; 

volatile bool mpuInterrupt = false; 

int heading;
// sonar
const int trigPin = 48;
const int echoPin = 46;
const int MAX_DISTANCE = 200;
NewPing sonar(trigPin, echoPin, MAX_DISTANCE);
int readDistance = 200;
// servo
Servo servo_motor; 
int pos = 0;
void dmpDataReady()
{
  //The MPU6050 sends out an interrupt every time there’s new data available. But the interrupt must only be enabled after the DMP is done. Hence, this part:
  mpuInterrupt = true;
}

void GetMPUValue(){
// if programming failed, don't try to do anything
  if (!dmpReady) return;

  // while (!mpuInterrupt && fifoCount < packetSize);

   // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount(); // get current FIFO count

  // check for overflow (this should never happen unless our code is too inefficient)
   if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    // Serial.println(F("FIFO overflow!"));
 
  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  }
  else if (mpuIntStatus & 0x02)
  {
    
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    
    fifoCount -= packetSize;
 
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    Serial.print("Yaw: ");
    Serial.println(ypr[0] * 180/M_PI);
    // Serial.print("Pitch: ");
    // Serial.println(ypr[1] * 180/M_PI);
    // Serial.print("Roll: ");
    // Serial.println(ypr[2] * 180/M_PI);
    mpuValue = (ypr[0] * 180/M_PI);
    if (mpuValue < 0) {
      mpuValueNegative = true;
    } else {
      mpuValueNegative = false;
    }
    // mpuValue = abs(mpuValue); //Concert to positive.
    
  }

}
int readPing() { // this read the distance of object
  delay(70);
  int cm = sonar.ping_cm();
  if(cm==0)
  {
    cm = 250;
  }
  return cm;
}

void AdjustLeft(){
  //Viewed from behind BOE BOT Car...
  Serial.println("AdjustLeft..."); //Debug.
  // int modifiedmpuValue = 5; //Was 50, 25.
  // turn on motor A
  digitalWrite(ava1, HIGH);
  digitalWrite(ind1, LOW);
  LeftWheelSpeed = 80;

   // turn on motor B
  digitalWrite(ava2, LOW);
  digitalWrite(ind2, HIGH);//
  RightWheelSpeed = 50;

  // LeftWheelSpeed = LeftWheelSpeed + modifiedmpuValue;  //Speed up wheel speed.

  // RightWheelSpeed = RightWheelSpeed - modifiedmpuValue; //Slow down wheel speed.

  if(LeftWheelSpeed > 80)
  {
    LeftWheelSpeed = 80;
  } //Don't exceed max speed value of 1100.

  if(RightWheelSpeed > 50)
  {
    RightWheelSpeed = 50;
  }//Don't reach stop value
  // set speed to 200 out of possible range 0~255
  analogWrite(motor1_speedPin, LeftWheelSpeed);
  analogWrite(motor2_speedPin, RightWheelSpeed);
}

void AdjustRight(){
  //Viewed from behind BOE BOT Car...
  Serial.println("AdjustRight..."); //Debug.
  // int modifiedmpuValue = 5; //Was 50, 25.
   // turn on motor A
  digitalWrite(ava1, HIGH);//
  digitalWrite(ind1, LOW);
  LeftWheelSpeed = 50;

  // turn on motor B
  digitalWrite(ava2, LOW);
  digitalWrite(ind2, HIGH);
  RightWheelSpeed = 80;
  // LeftWheelSpeed = LeftWheelSpeed - modifiedmpuValue;  //Slow down wheel speed.
  // RightWheelSpeed = RightWheelSpeed + modifiedmpuValue; //Speed up wheel speed.
 if(LeftWheelSpeed > 60)
  {
    LeftWheelSpeed = 50;
  } //Don't exceed max speed value of 1100.

  if(RightWheelSpeed > 80)
  {
    RightWheelSpeed = 80;
  }//Don't reach stop value

  // set speed to 200 out of possible range 0~255
  analogWrite(motor1_speedPin, LeftWheelSpeed);
  analogWrite(motor2_speedPin, RightWheelSpeed);
}


void moveForward()
{
 Serial.print("Going Forward\n");
   // turn on motor A
  digitalWrite(ava1, HIGH);
  digitalWrite(ind1, LOW);
  // set speed to 200 out of possible range 0~255
  analogWrite(motor1_speedPin, 80);
  // turn on motor B
  digitalWrite(ava2, LOW);
  digitalWrite(ind2, HIGH);
  // set speed to 200 out of possible range 0~255
  analogWrite(motor2_speedPin, 80);
}
void stopCorrect()
{
 Serial.print("STOPPPPP\n");
   // turn on motor A
  digitalWrite(ava1, LOW);
  digitalWrite(ind1, LOW);
  // set speed to 200 out of possible range 0~255
  analogWrite(motor1_speedPin, 0);
  // turn on motor B
  digitalWrite(ava2, LOW);
  digitalWrite(ind2, LOW);
  // set speed to 200 out of possible range 0~255
  analogWrite(motor2_speedPin, 0);
  delay(100);
}
void setup() {
Serial.begin(9600);
// Motor Stuff
pinMode(motor1_speedPin, OUTPUT);
pinMode(ava2, OUTPUT);
pinMode(ind2, OUTPUT);
pinMode(motor2_speedPin, OUTPUT);
pinMode(ava1, OUTPUT);
pinMode(ind1, OUTPUT);
servo_motor.attach(11);
pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
// join I2C bus (I2Cdev library doesn't do this automatically)
#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

// Initialization involves several steps including waking up the device. The DMP also needs to be initialized and the function that does this returns a flag to tell the status:
mpu.initialize();
devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXAccelOffset(-1063); //1293,
  mpu.setYAccelOffset(75); //2425,
  mpu.setZAccelOffset(1609); // 1572, (1688 factory default for my test chip)
  mpu.setXGyroOffset(103);  //-13,
  mpu.setYGyroOffset(19);  //-55,
  mpu.setZGyroOffset(21);  //-27,

// make sure it worked (returns 0 if so)
  if (devStatus == 0) // If the DMP successfully initialized, devStatus will be zero, hence:
  {
    // turn on the DMP, now that it's ready
    mpu.setDMPEnabled(true);
 
    // enable Arduino interrupt detection
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
 
    
    dmpReady = true;
 
    
    packetSize = mpu.dmpGetFIFOPacketSize();
    
  }
  else 
  {
       
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }


  delay(3000); // Wait 15 - 40 seconds to allow setup of MPU6050.
  mpuValue = 0; //Initialize yaw to zero in order to bypass first heading check in Loop.
  heading = 0; //Initialize to zero.
  readDistance = 200;
}

void loop() {
GetMPUValue();
readDistance = readPing();

if (readDistance < 20){
    Serial.print("STOP OBSTACLE");
    stopCorrect();
}
  else {
    GetMPUValue();
    if (mpuValue >= -0.8 && mpuValue<= 0.8)
    {
    moveForward();
    } else if (mpuValue<-0.8){
    AdjustRight();
    }else if (mpuValue>0.8){
    AdjustLeft();
    }
  }
GetMPUValue();
readDistance = readPing();
}

For that purpose it would be better to use a tilt compensated compass, since with the MPU6050, yaw is relative to the starting value and will always drift.

Most people use PID to have the robot to move in a straight line, along a desired bearing. The basic idea is outlined below. It assumes that yaw (bearing) increases from North to East, as standard for navigation.

void loop() {
heading = get_yaw();  //get current heading
yaw_error = (heading-bearing); //bearing is the setpoint or desired heading
set_left_motor_speed(base_speed - Kp*yaw_error); //if error is positive, slow down left motor
set_right_motor_speed(base_speed + Kp*yaw_error); //maintain speed by speeding up right motor
}

It will take a few trials to get Kp right. Start with 1.0 (google "PID tuning" for details).

You seem to be using speeds of 80, 80 to go straight and 50, 80 to go both left and right?!? I can see how slowing the left wheel would turn left but I can't see how slowing the left wheel can also turn right.

johnwasser:
You seem to be using speeds of 80, 80 to go straight and 50, 80 to go both left and right?!? I can see how slowing the left wheel would turn left but I can't see how slowing the left wheel can also turn right.

Three left turns make a right turn?