Measure distance and angle turned in a robot car MPU6050

Hi, I am developing a project where the user transmits certain distances and angles and the robot will have to follow this path. What I am trying to figure out now is how to make the robot turn a specified angle. I purchased a gyroscope module (MPU6050) for this task but I am having problems making it work. Can someone please assist me on how to use this module to accomplish this task? Else if there are any better modules/sensors that could accomplish this task a lot easier. Any help would be much appreciated

I have no practical experience with such devices yet, but I fear that they have an accuracy problem. Depending on a couple of effects, the robot may deviate more and more from the given path. I'd see their use more in steering, where the user tilts or turns a tablet or game console, to make an car, robot or drone move in the same way, so that the operator provides the feedback when the intended direction or position really has been reached. Like you move a computer mouse until the cursor on screen reaches the intended position, not by positions on the mouse pad.

Any comments, anybody?

well i was thinking the accuracy can be accounted for by setting a tolerance. the way i see it this is the pseudo code of what it should be like

while currentangle!=desiredangle
turn the robot
Stop turning

anyone know how i can get this done?

Post the code, using code tags (</> button), that you are using with the MPU6050 sensor.

To measure distance you will need wheel encoders.

The sensors typically supply linear accelerations or rotational speed. For linear speed, position or angle outputs the signals must be integrated, and that's the weak point I know from old (mechanical gyro) devices. Based on the basic sensor values it's possible to detect still stand (rotational), and declination vs. the Z axis (gravity). For absolute Z angles a compass module can be used, but frequently EM fields from the environment make the values unreliable. For absolute coordinates GPS could be used, but the resolution is typically sufficient for long distance navigation only, not for indoor applications with small targets.

You need more than a gyroscope. You will need wheel encoders that measure the angle each wheel turns. You can calculate robot distance and direction using info from the encoders. A platform like the Lego NXT has these built in.

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
    #include "Wire.h"
MPU6050 mpu;
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;

// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
        Fastwire::setup(400, true);

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    while (!Serial); // wait for Leonardo enumeration, others continue immediately


   devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setZAccelOffset(1682); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {


        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

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

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
        // other program behavior stuff here
        // .
        // .
        // .
        // if you are really paranoid you can frequently test in between other
        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
        // while() loop to immediately process the MPU data
        // .
        // .
        // .

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

// display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print(euler[0] * 180/M_PI);
//            Serial.print("\t");
//            Serial.print(euler[1] * 180/M_PI);
//            Serial.print("\t");
//            Serial.println(euler[2] * 180/M_PI);

        // blink LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);

that is the code that I am using for the MPU6050. its quite complex so not sure how to obtain the angle and use it for the task needed. any help on this?

Moderator edit: Code tags. It’s always code tags. Why?

Didn't the section titled

// display Euler angles in degrees

ring any bells? Part of it is commented out.

I actually managed to get the angle sensor working. turns out the code i want to insert has to be placed in that while loop. but the problem is its difficult to get it to turn an accurate angle since it tends to overshoot the angle and makes one full rotation again till it gets it right. any ways of fixing this?

On a seperate note can someone assist me in how to use the encoders to measure distance? I have a code that measures rpm of the encoder disk. how to i use this to obtain distance travelled?

You probably need to apply a PID algorithm for turning, to prevent "overshoot".

No matter how precise you try to make this system, there will -always- be error in the system that you cannot overcome. Instead of working to remove it - take it into account, and work with it.

I am not very familiar with using PID algortithms. Any pointers as to how I would go about implementing it crosh?