Tuning PD for line follower

Question

I am trying to make line following robot. I am using atmega328p mcu, pololu 10:1 motors, pololu qtr6-rc sensor, 2s li-po. Here is my code:

/*
* LineFollower.c
*
* Created: 30.04.2015 16:00:05
*  Author: Mikk
*/
#define F_CPU 20000000         //we're running on 20mHz clock

#define numberOfButtons 1

#define READPORT    PORTC
#define READDDR     DDRC
#define READPIN     PINC        // lines connected to PC0 - PC5

#define MAXTICKS    2500
#define QTRCNT      6

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <Mikk/Button.h>
#include <Mikk/QTRRCSensors.h>

int baseSpeed = 70;
int maxSpeed = 140;
const float Kp = 8.1;
const float Kd = 400;

uint8_t mode = 0;                //indicates in which mode program is 

uint8_t RmotorSpeed = 0;         //
uint8_t LmotorSpeed = 0;         //motors

void button(void);

void setMotors(int ml, int mr)
{
    if(ml > maxSpeed)             //make sure that speed is not out of range for left motor
        ml = maxSpeed;
    if(ml < -maxSpeed)
        ml = -maxSpeed;

    if(mr > maxSpeed)             //make sure that speed is not out of range for right motor
        mr = maxSpeed;
    if(mr < -maxSpeed)
        mr = maxSpeed;

    if(ml > 0)                    //if left motor speed is positive then drive motor forwards
        LmotorSpeed = ml;
    if(ml == 0)                   //if left motor speed is 0 then stop motor
        LmotorSpeed = 0;

    if(mr > 0)                    //if right motor speed is positive then drive motor forwards
        RmotorSpeed = mr;
    if(mr == 0)                   //if right motor speed is 0 then stop motor
        RmotorSpeed = 0;
}

void emittersOn(void)            //function for turning emitters on
{
    PORTD |= (1 << PIND0);
}

void emittersOff(void)           //function for turning emitters off
{
    PORTD &= ~(1 << PIND0);
}

void LedOn(void)                 //function for turning led on
{
    PORTB |= (1 << PINB5);
}

void LedOff(void)               //function for turning led off
{
    PORTB &= ~(1 << PINB5);
}

void stop(void)                 //stop everything
{
    LedOff();
    setMotors(0, 0);
    emittersOff();
}

void calibration(void)          //calibration takes about 5 seconds
{
    //turn led on
    LedOn();
    //turn emitters on
    emittersOn();
    // reset minimums and maximums
    for (int i = 0; i < QTRCNT; i++)
    {
        QTRmax[i] = 0;
        QTRmin[i] = MAXTICKS;
    }
    //calibrate sensors
    for(int i=0; i<250; i++)
    {
        calibrateQTRs();
        _delay_ms(5);
    }
    //turn emitters off
    emittersOff();
    //turn led off
    LedOff();
}

void start(void)
{
    //turn led on
    LedOn();

    //create all necessary variables
    int power_difference = 0;
    float error = 0;
    float lastError = 0;
    float derivative = 0;
    int position = 0;

    //turn emitters on
    emittersOn();
    _delay_ms(500);               //wait so you can pull your hand away
    while(mode == 2)
    {
        //check for mode change
        button();

        //read position
        position = readLine();
        //make calculations
        error = position - 2500;
        derivative = error - lastError;

        //remember last error
        lastError = error;

        //calculate power_difference of motors
        power_difference = error/(Kp/100) + derivative*(Kd/100);

        //make sure that power difference is in correct range
        if(power_difference > baseSpeed)
            power_difference = baseSpeed;
        if(power_difference < -baseSpeed)
            power_difference = -baseSpeed;

        //drive motors
        if(power_difference > 0)
            setMotors(baseSpeed+power_difference, baseSpeed-power_difference/2);
        else if(power_difference < 0)
            setMotors(baseSpeed+power_difference/2, baseSpeed-power_difference);
        else if(power_difference == 0)
            setMotors(maxSpeed, maxSpeed);
    }
}

void button(void)
{
    char buttonState = 0;
    //check for current button status
    buttonState = ButtonReleased(0, PINB, 1, 200);
    //check if button is pressed
    if(buttonState) //pin change from low to high
    {
        mode++;
        if(mode == 1) calibration();
    }
}

void pwmInit(void)
{
    //set fast-PWM mode, inverting mode for timer0
    TCCR0A |= (1 << COM0A1) | (1 << COM0A0) | (1 << WGM00) | (1 << WGM01) | (1 << COM0B1) | (1 << COM0B0);
    //set fast-PWM mode, inverting mode for timer2
    TCCR2A |= (1 << COM2A1) | (1 << COM2A0) | (1 << WGM20) | (1 << WGM21) | (1 << COM2B1) | (1 << COM2B0);
    //set timer0 overflow interrupt
    TIMSK0 |= (1 << TOIE0);
    //set timer2 overflow interrupt
    TIMSK2 |= (1 << TOIE2);

    //enable global interrupts
    sei();
    //set timer0 prescaling to 8
    TCCR0B |= (1 << CS01);
    //set timer2 prescaling to 8
    TCCR2B |= (1 << CS21);
}

int main(void)
{   
    DDRB |= 0x2A;                 //0b00101010
    DDRD |= 0x69;                 //0b01101001
    DDRC |= 0x00;                 //0b00000000

    //clear port d
    PORTD |= 0x00;

    //enable pull-up resistor
    PORTB |= (1 << PINB1);

    initQTRs();
    pwmInit();

    //blink 2 times indicate that we are ready
    for(int i=0; i<4; i++)
    {
        PORTB ^= (1 << PINB5);
        _delay_ms(500);
    }

    while(1)
    {
        button();
        if(mode == 0) stop();
        if(mode == 2) start();
        if(mode >= 3) mode = 0;
    }
}

//update OCRnx values
ISR(TIMER0_OVF_vect)
{
    OCR0A = RmotorSpeed;
}
ISR(TIMER2_OVF_vect)
{
    OCR2A = LmotorSpeed;
}

And here is my qtr library:

    #ifndef QTRRCSensors
#define QTRRCSensors

#define SLOW        1
#define FAST        0

static inline void initQTRs(void) 
{
    TCCR1B = (1 << CS11);
}

uint16_t QTRtime[QTRCNT], QTRmax[QTRCNT], QTRmin[QTRCNT];

static inline void readQTRs(uint8_t forceSlow) {
    uint8_t lastPin, i, done = 0;

    for (i = 0; i < QTRCNT; i++)                    // clear out previous times
        QTRtime[i] = 0;

    READDDR |= 0b00111111;                          // set pins to output
    READPORT |= 0b00111111;                         // drive them high

    _delay_us(10);                                  // wait 10us to charge capacitors

    READDDR &= 0b11000000;                          // set pins to input
    READPORT &= 0b11000000;                         // turn off pull-up registers

    TCNT1 = 0;                                      // start 16bit timer at 0
    lastPin = READPIN;

    while ((TCNT1 < MAXTICKS) && ((done < QTRCNT) || forceSlow))     // if forceSlow, always take MAXTICKS time
    {
        if (lastPin != READPIN)                     // if any of the pins changed
        {
            lastPin = READPIN;
            for (i = 0; i < QTRCNT; i++)
            {
                if ((QTRtime[i] == 0) && (!(lastPin & (1<<i))))    // did pin go low for the first time
                {
                    QTRtime[i] = TCNT1;
                    done++;
                }
            }
        }
    }
    if (done < QTRCNT)                              // if we timed out, set any pins that didn't go low to max
        for (i = 0; i < QTRCNT; i++)
            if (QTRtime[i] == 0)
                QTRtime[i] = MAXTICKS;
}

void calibrateQTRs(void) {
    uint8_t i, j;

    for (j = 0; j < 10; j++) {                      // take 10 readings and find min and max values
        readQTRs(SLOW);
        for (i = 0; i < QTRCNT; i++) {
            if (QTRtime[i] > QTRmax[i])
                QTRmax[i] = QTRtime[i];
            if (QTRtime[i] < QTRmin[i])
                QTRmin[i] = QTRtime[i];
        }
    }
}

void readCalibrated(void) {
    uint8_t i;
    uint16_t range;

    readQTRs(FAST);

    for (i = 0; i < QTRCNT; i++) {                  // normalize readings 0-1000 relative to min & max
        if (QTRtime[i] < QTRmin[i])                 // check if reading is within calibrated reading
            QTRtime[i] = 0;
        else if (QTRtime[i] > QTRmax[i])
            QTRtime[i] = 1000;
        else {
            range = QTRmax[i] - QTRmin[i];
            if (!range)                             // avoid div by zero if min & max are equal (broken sensor)
                QTRtime[i] = 0;
            else
                QTRtime[i] = ((int32_t)(QTRtime[i]) - QTRmin[i]) * 1000 / range;
        }
    }
}

uint16_t readLine(void) {
    uint8_t i, onLine = 0;
    uint32_t avg;                                   // weighted total, long before division
    uint16_t sum;                                   // total values (used for division)
    static uint16_t lastValue = 0;                  // assume line is initially all the way left (arbitrary)

    readCalibrated();

    avg = 0;
    sum = 0;

    for (i = 0; i < QTRCNT; i++) {                  // if following white line, set QTRtime[i] = 1000 - QTRtime[i]
        if (QTRtime[i] > 50) {                      // only average in values that are above a noise threshold
            avg += (uint32_t)(QTRtime[i]) * (i * 1000);
            sum += QTRtime[i];
            if (QTRtime[i] > 200)                   // see if we're above the line
                onLine = 1;
        }
    }

    if (!onLine)
    {
        // If it last read to the left of center, return 0.
        if(lastValue < (QTRCNT-1)*1000/2)
            return 0;

        // If it last read to the right of center, return the max.
        else
            return (QTRCNT-1)*1000;

    }

    lastValue = avg/sum;                            // no chance of div by zero since onLine was true

    return lastValue;
}

#endif

I am trying to find Kp constant but when it's 7 then my robot just turns off the line always on the same spot. When Kp is 8 then it follows staright line but wobbles a lot and can't take corners. I also tried to increase Kd 10 to 20 times when my Kp was 8 but it didn't change much. How can I get it working?

Here is my robot and the track I want to follow.

Answer 1

Your PID calculation is all wrong. You have:

 //calculate power_difference of motors
power_difference = error/(Kp/100) + derivative*(Kd/100);

First, you need correct error terms. You calculate:

//make calculations
error = position - 2500;
derivative = error - lastError;

I'm assuming here that 2500 is the signal you are expecting if you are exactly centered on the line. Even if this is the case, you should be calculating error as:

error = reference - output; 

You have, assuming 2500 is the ideal line reading,

error = output - reference; 

which is incorrect. This is flipping the sign on your error term. You then calculate derivative error as the difference between current and previous error, which is also incorrect. You need the sample time, loop time, sweep time, etc. - the time that elapses between when you calculate the error terms. You can do this with a timer for an exact measurement or you can use a pause or delay command at the end of the loop for an approximate measurement.

Once you have your sample time, your derivative error is calculated by:

derivativeError = (error - previousError)/sampleTime;

Now your control signal is just proportional gain times error and derivative gain times derivative error. You are dividing by your gains and then dividing those gains each by 100.

Where you have:

power_difference = error/(Kp/100) + derivative*(Kd/100);

You should have:

power_difference = error*Kp + derivative*Kd;

Honestly I'm surprised you managed to get it to do anything at all. Putting it all together, I would suggest something like:

sampleTime = 50;    // 50 ms sample time
while(mode == 2)
{
    //check for mode change
    button();

    //read position
    position = readLine();
    //make calculations
    error = 2500 - position;
    derivative = (error - lastError)/sampleTime;

    //remember last error
    lastError = error;

    //calculate power_difference of motors
    power_difference = error*Kp + derivative*Kd;

    //make sure that power difference is in correct range

    //drive motor 
    delay (sampleTime)
}

Adjust sample time down if you find you absolutely need a faster response, but know that the lower you set it the more inaccurate you get because calculation time takes up a proportionally bigger time.

Finally, reset your gains and tune again. Check out Ziegler-Nichols method for some help tuning your controller once you've made the code changes.

Answer 2

When looking at PID loop control code, if statements are always a code smell for me. Every if statement has the potential to add a discontinuity into the control loop that complicates PID tuning immensely. PID loops work much more predictably when their response is smooth (within the bounds of the limits of the system).

Unfortunately, your two main functions, start and setMotors have lots of ifs and thus several potential discontinuities.

setMotors()

If either motor ever gets to max. speed, it is clamped at max. speed without changing the speed of the other motor. If you consider how your robot moves, if one motor is running slower than has been calculated, the other by definition is running faster than it should be for your calculated power difference and you will be moving in the wrong direction. The only way to avoid this is to tune your system so conservatively that you never ask motors to run at maxSpeed.

This isn't helped by the fact that if (mr < -maxSpeed) sets mr = maxspeed rather than mr = -maxspeed which could be rather unfortunate - driving that motor at max. speed in the opposite direction. *8')

Also, if start calculates that a motor should be run in reverse you are throwing away that command and leaving it running at whatever speed it was running before! While start shouldn't do this at the moment, that may not always be the case. If you changed start to better deal with the right angle turn in your track for instance, it could easily require negative speeds for one motor to turn on the spot.

This could have serious repercussions for the stability of your robot.

start()

Another example of a discontinuity is in start, where as the absolute value of power_difference tends towards 0, the both motor speed converges on baseSpeed, but as soon as power_difference hits 0, both motors jump to maxSpeed. Doubling your speed like this can play havoc with with your tuning, and cause you to need to tune much more conservatively to avoid instability.

All it takes is for one motor to respond slightly more aggressively to the command than the other and you have been kicked off in the wrong direction and then your subsequent PID cycles will have to compensate.

Suggestions

I would suggest that you try to rework your calculations so that you have a continuum between maxSpeed through baseSpeed and 0 to -maxSpeed based on the magnitude of the error.

I would also start by making sure that you can follow the track slowly with conservative tuning and then ramp up to higher speeds and more aggressive turns as you get more experienced with running your PID loop.

See also my answer to Line Follower optimization.