Implementing PID with IMU as input

Using Arduino Programming Questions
1

I understand if people don't want to help with this because I know there are a LOT of posts on this subject, but I've been reading examples all day and for some reason it's just not clicking. That being said, any and all help is greatly appreciated.

I have a robot with 2 motors being driven by an l298d. The front wheels are casters that go where the back wheels point them. If I give power to the back right wheel, it turns left and vice versa. I want to implement PID to make it go straight using the initial yaw reading (obtained after 15 seconds of the IMU - a GY-521 - running) as my desired heading (setpoint). The input of the PID needs to be the current yaw reading, and the output needs to be to the appropriate motor.

I found what should be excellent forum posts like this one. Or from another site such as here. I genuinely understand the concept of PID, but how to implement it programming wise is just completely going over my head as of now.

For a bit of context, here is what I am working with at the moment.
(code added as attachment due to max character limit - including attempt to define setpoint at end of setup)

Almost all of that was taken from a sketch called MPU6050_DMP6 by Jeff Rowberg. Pretty much the only stuff I added was there at the end under the NEW section. And that part does what I expect it to. The first thing I'm really struggling with is how to get a "snapshot" of the yaw reading to then use as the setpoint. If I do for example:

double setpoint
setpoint = ypr[0] * 180/M_PI;

, then that would be constantly changing my setpoint to whatever the current reading is, correct (and that is actually what I want my input to be)? Furthermore, where exactly to define the setpoint is confusing me. The best attempt I have is adding this to the end of the setup function (included in attached code), but it just returns 180.00 every time!

    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    double Setpoint;
    delay(15000);
    Setpoint=ypr[0] * 180/M_PI;
    Serial.print(Setpoint);

As you can tell, I'm pretty lost at the moment, and any help at all is greatly appreciated.

DMP6_Reduced_motorcheck.ino (7.93 KB)

2 
    double Setpoint;

You declared Setpoint locally. It goes out of scope when setup() finishes. Make it global instead.

3

Alright I did that, but no change. I think because as of now the only time I'm using it is also in the setup function.

4

PID? Looks to me like you're implementing a "bang bang" controller instead.

A "bang bang" is either on or off (well, in your case, left right, or straight) instead of a smooth analog(ish) output.

If you want to make a PID loop, you need proper feedback. I'll assume the IMU will be good enough at measuring the correct yaw angle (although I doubt it will be unless you've already calibrated it). Even still, you have to worry about not only yaw angle, but distance away from the intended path. That is, unless you only care about it going straight and not following a specific path.

Once the feedback problem is resolved, you input the feedback data and setpoint into a PID controller in an iterative fashion. In your case, you can easily use the Arduino built in library PID.h. The controller will then provide the Arduino with how fast (and in what direction) the motors should spin.

Note that you will need to tune the P, I, and D parameters - its just guess and check tbh.

5

Yes, you're right - what I have there is a "bang bang" approach. I want to replace that with a PID loop. The IMU will be good enough for now. Soon I will get new motors with encoders, and further in the future either a laser range finding system or a good ultrasonic sensor. In the end, I want to have them all working together - kind of a checks and balances system. For now, distance away from intended path isn't much of a concern and I'd like to just set up the PID loop with the IMU as the input. Do you know how I can do that? I know I'll use the PID library, but like I said earlier, I don't even know how to get the initial setpoint from the sensor. I'm able to get continuous readings, but how do I get a "snapshot" so to speak to use as the setpoint? For example, when I put the robot down, I want the IMU to run for 15 seconds until the readings settle, then whatever value the yaw is at that point will be my straight line (setpoint).

6

KitNo:
I don't even know how to get the initial setpoint from the sensor. I'm able to get continuous readings, but how do I get a "snapshot" so to speak to use as the setpoint? For example, when I put the robot down, I want the IMU to run for 15 seconds until the readings settle, then whatever value the yaw is at that point will be my straight line (setpoint).

You won't like my answer, but here it is: it's however you want to find the setpoint - there is no right or wrong way.

Now, how to figure out the best way for you to find the setpoint depends on what the vehicle is supposed to do, what it's limitations are, and what it's operating conditions will be. What is it for? Can you give us a complete project description? With this info we can help figure out the best way to find setpoints.

7

It's intended to be a lawn mower.

You won't like my answer, but here it is: it's however you want to find the setpoint - there is no right or wrong way.

I think you misunderstand when I say I don't know how to get the setpoint for the PID. There is definitely a right way here. It just needs to be whatever the yaw reading is when I put it on the ground. I'm going to put it down facing the direction I want it to go, and whether that reading is -120 or 65 or 2 (the gy-521 I have doesn't have a magnetometer so it's not based on true north, and it doesn't need to be) is irrelevant. That's just the way it's pointing, and is therefore what I want the setpoint to be.

8

KitNo:
I think you misunderstand when I say I don't know how to get the setpoint for the PID. There is definitely a right way here. It just needs to be whatever the yaw reading is when I put it on the ground.

Ok, cool. So the real question is "How do I write the code so that I can average the yaw readings for 15 seconds after putting the lawn mower down and make that average my setpoint?" Is that what you want to know?

9

Almost! But I don't want the average, I want the reading after 15s has elapsed (the first 15s after it starts reading is kind of a calibration period. The readings will trend one way and then settle some time around 15s). Like I said I know how to read this value, but it's dynamic - changing all the time when the sensor moves. I need to know how to store it and use it as the setpoint for the PID. Then if I can get a basic framework for the PID (where to define which variables, etc.), the input will be the current yaw reading, and the output will be to send more/less power to the appropriate motors.

10

KitNo:
Like I said I know how to read this value, but it's dynamic - changing all the time when the sensor moves.

Sounds like you could use some averaging then. What I would suggest is having a momentary button. Design it to where after you set the mower down, you press the button, it waits 15 seconds (could use delay(15000) if you want to) and then take 50 readings. Average those readings and then save that value in a variable that's passed to the PID controller as your setpoint. Then make sure the setpoint isn't updated until the next time the button is pressed.

Also, you'll have more luck if you initialize your setpoint like this:

float setpoint;
setpoint = averageYawAngle * 180.0/M_PI;
11

To steer something using a compass and PID, the simplest approach is to use Setpoint=0 and Input= (Heading Error) = (desired heading) - (current heading).

Degrees is fine for most robots, but you have to be careful with compass wrap, that is, if the desired heading is 2 degrees from North and the current heading is 358 degrees, you want to correct the four degree error by turning right, not the 356 degree error by turning left.

A handy statement function to take care of that ambiguity is:

int heading_error = (540 + desired_heading - current_heading)%360-180;  //range -180 to 180 in degrees

Then the simplest possible PID steering correction is (with Kd = Ki = 0):

output = Kp*heading_error;

Finally, since you are usually moving more or less straight ahead at base_speed, you want to modify the relative speeds of the left and right motors in order to turn gradually:

set_left_motor_speed(base_speed + output); //veer right if correction is positive
set_right_motor_speed(base_speed - output);

Lastly, adjust Kp as needed, and possibly add Ki and Kd.

12

This is a great reply jremington! I've actually had to deal with compass wrap on a separate issue before, and didn't even think about how I was going to deal with it here yet. On the other issue I added 360 to all values but that compass went from 0-360 rather than -180 to 180, so I'm sure you're spot on there (just going to have to think about it here in a bit when the coffee kicks in!) I've begun adding in the ideas you suggested and run into the same brick wall I was at before. I think if I knew how to ask my question better I probably could've had it answered here very quickly, but I just don't know the vernacular. I need to know how to read AND THEN STORE that value so that it doesn't change when the IMU moves.

int heading_error = (540 + desired_heading - current_heading)%360-180;  //range -180 to 180 in degrees

How do I define desired_heading? I simply don't know the syntax .. I'm thinking something like

DesiredHeading = digitalRead(ypr[0]*180/M_PI);

But that's not right. Also, in this updated attempt, I defined the PID variables before setup(), and made a new PID_Loop() to assign them values. I also tried to print them to the serial monitor, but nothing is being printed. In addition, I removed my old bang bang approach as that was really just a testing thing to make sure I could read from the IMU and make a motor do something at all. Any idea why nothing is being printed in the serial monitor? Did I define things in the right place (I know they probably shouldn't be integers, but as long as it will round for me that's fine for now, or I can change them to float if you think I should now)?

All in all, I understand much more than I did and really like your approach, just having difficulty programming it.

DMP6_Reduced_motorcheck.ino (8.17 KB)

13

digitalRead() just reads the state of one of the Arduino's physical pins. It does not belong here.

The rest of your code did not attach properly. If possible, trim your code down to the smallest size which will compile and show the problem. Then post it in-line in the forum. I can't open a .ino file on my phone and you will usually find that the trimming process finds the error for you.

14 
#include <PID_v1.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

MPU6050 mpu;

// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
//#define OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions coming
// from the FIFO.
#define OUTPUT_READABLE_YAWPITCHROLL

//////////////    NEW    ////////////////////////

#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 52 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
#define MRENA 22
#define MRDIRA 3
#define MRDIRB 4 
#define MLENA 48
#define MLDIRA 7
#define MLDIRB 8

//Setting up PID values

double Setpoint, Input, Output;       //creating variables to hold PID values
int CurrentHeading;
int DesiredHeading;
int HeadingError;

/////////////////////////////////////////////////////

// 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
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)
    Wire.begin();
    Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties

    Serial.begin(38400);

    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();
    pinMode(INTERRUPT_PIN, INPUT);

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // wait for ready
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(0);
    mpu.setYGyroOffset(10);
    mpu.setZGyroOffset(-3);
    mpu.setZAccelOffset(1688); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
        Serial.println(F(")..."));
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), 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 "));
        Serial.print(devStatus);
        Serial.println(F(")"));  
    }
    
}

// ================================================================
// ===                    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) 
    {
        if (mpuInterrupt && fifoCount < packetSize) 
        {
          // try to get out of the infinite loop 
          fifoCount = mpu.getFIFOCount();
        }    
    }

    // 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 & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        fifoCount = mpu.getFIFOCount();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
        // 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;

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            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);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif
   }

 //////////////////                 NEW             ////////////////////////////
  pinMode(LED_PIN, OUTPUT);
  pinMode(MLENA, OUTPUT);
  pinMode(MLDIRA, OUTPUT);
  pinMode(MLDIRB, OUTPUT);
  pinMode(MRENA, OUTPUT);
  pinMode(MRDIRA, OUTPUT);
  pinMode(MRDIRB, OUTPUT); 
}

void PID_Loop()
{
  Serial.begin(38400);
  delay(15000);
  CurrentHeading = ypr[0] * 180/M_PI;
  DesiredHeading = digitalRead(ypr[0] * 180/M_PI);
  Serial.println(DesiredHeading);
  HeadingError = (540 + DesiredHeading - CurrentHeading)%360-180;  //range -180 to 180 in degrees
  Serial.println(HeadingError);
  Setpoint = 0;
  Input = HeadingError - DesiredHeading - CurrentHeading;
  Serial.println(Input);
  PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
}
///////////////////////////////////////////////////////////////////////////////////
15

How do I define desired_heading

That is simply an integer (degrees from North) for the compass direction you want the robot to go. It is usually called the bearing angle.

If you want the robot to keep going in the direction that you initially point it, just read out the yaw angle and store that in desired_heading.

int desired_heading = read_compass_yaw();

Unfortunately, the MPU6050 does not have a magnetometer, so it is not possible to have stable yaw values -- they will drift with time even if you carefully calibrate the IMU.

For robot steering, I would recommend a compass module like the LSM303D, rather than an IMU. Be sure to calibrate the compass module after building in into the robot, or it won't work properly.

16

That is simply an integer (degrees from North) for the compass direction you want the robot to go. It is usually called the bearing angle.

If you want the robot to keep going in the direction that you initially point it, just read out the yaw angle and store that in desired_heading.

That's what I'm asking how to do. As of now, I can read the yaw angle with

DesiredHeading = (ypr[0] * 180/M_PI);

But how do I store it so that it does not change when the sensor moves. Whatever the reading is after the initial 15s calibration period needs to be stored and used as the setpoint for the PID. The only way that I know how to read it (shown above) changes when it moves. This is what I mean when I keep referring to taking a "snapshot" of the readings. I know that is the wrong term, but hopefully you know what I mean now.

Unfortunately, the MPU6050 does not have a magnetometer, so it is not possible to have stable yaw values -- they will drift with time even if you carefully calibrate the IMU

Like I said above, I'm aware it doesn't have a magnetometer and the yaw value isn't relative to true north - and it doesn't need to be. This is intended to be put on an automatic mower. With 8" drive wheels at 200rpm (will obviously be less when weighted down, hopefully down to about 50rpm) it will go between 400' (at 200rpm) and 100' (at 50rpm) in one minute. The drift as is configured is less than one degree every 4 minutes - meaning 1/4 degree in the one minute it will take to traverse my 100' lawn. This is an acceptable level of error for me FOR NOW - around 6 inches off over the 100'. After it travels 100', it will start a 180deg left turn, and take whatever the yaw reading is (which will negate the drift) as it's new DesiredHeading, travel another 100' back the other way, make a 180deg right turn, and start over.

P.S.: I have tried experimenting with the Using millis() for timing sticky on this forum to time out the 15s calibration period. Not much luck yet, but I will post what I have pretty soon.

17

I found some VERY relative code from here. Including a 15s wait calibration period and everything. Unfortunately he's using some H files I'm not familiar with and cant upload it to my arduino because of the error "no matching function for call to 'PID::PID()'" and "'class PID' has no member named 'Setup'". Hopefully someone else can make heads or tails of what he's doing there and apply it to my situation. I'll be working on it .. (It was too long to put in code markers)

_2020Bot_MPU6050.ino (11.9 KB)

18

But how do I store it so that it does not change when the sensor moves.

It will not change unless you change it.

At program start, use this line:

DesiredHeading = (ypr[0] * 180/M_PI);

Later, use this line:

CurrentHeading = (ypr[0] * 180/M_PI);

If you have to struggle with such very basic concepts, you need to go back to near the beginning and learn the programming language.

19

If I put it at the beginning then the sensor won't have even been initialized, nor would 15s have passed. That's why I'm here asking for help. From my very first post:

Furthermore, where exactly to define the setpoint is confusing me. The best attempt I have is adding this to the end of the setup function (included in attached code), but it just returns 180.00 every time!

So where is "program start"? Where do I have the opportunity for the sensor to have been running 15s to then capture its reading and set it?

If you have to struggle with such very basic concepts, you need to go back to near the beginning and learn the programming language.

Like I said, all help is greatly appreciated, and you have been helpful in many ways. This is not one of them. I'm asking for programming help on a forum called "Programming Questions". Do you really think "you need to go back to near the beginning and learn the programming language" is helpful in any way?

20

KitNo:
After it travels 100', it will start a 180deg left turn, and take whatever the yaw reading is (which will negate the drift) as it's new DesiredHeading,

It won't remove the drift because the only way you have to measure the 180 turn is using the drifting gyro. But it still seems like your plan could work to an acceptable accuracy.

Just put a delay() in setup and then calulate DesiredHeading there.