# Tuning PD for line follower

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.

## 2 Answers

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.

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.