PID based Line Following using Pololu QTR-8RC Sensors

Hello there,

I am using a Arduino ATMEGA168 clone along with the 30:1 MP micrometal gear motors, the pololu tb6612fng motor driver, and of course, the Pololu QTR-8RC sensor. I have got the robot up and running, motor control flawless, and now i am shifting towards PID based line following. I have built some line followers, but they were not PID based, nor had i made use of the QTR sensors. I will post my code below, which, i don’t think is correct to some extent. :confused:

I went through the wonderful PDF explanation of using the QTR sensors, but am encountering problems on my way.

The line width for a competition i am taking part is 2.5 cm, therefore, i am using sensor 4 as the middle sensor, and sensors 1 and 2 as right sensors and 6 and 7 as left sensors.

Now the problem is, i have set my base motor speeds at a PWM value of 10. I serial print the final motor speeds ( m1speed and m2speed as mentioned in the PDF), they are sometimes at 5000, sometimes at -270, its all random. Surely, it has to do something with the code. Thus, the bot is completely random.

#include <QTRSensors.h>

#define KP .2
#define KD 5
#define M1_DEFAULT_SPEED 10
#define M2_DEFAULT_SPEED 10
#define MIDDLE_SENSOR 4
#define NUM_SENSORS  5      // number of sensors used
#define TIMEOUT       2500  // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN   2     // emitter is controlled by digital pin 2
#define DEBUG 1 // set to 1 if serial debug output needed

#define rightMotor1 3
#define rightMotor2 4
#define rightMotorPWM 5
#define leftMotor1 12
#define leftMotor2 13
#define leftMotorPWM 11
#define motorPower 8

QTRSensorsRC qtrrc((unsigned char[]) {  14, 15, 16, 17, 18} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];

void setup()
{
  pinMode(rightMotor1, OUTPUT);
  pinMode(rightMotor2, OUTPUT);
  pinMode(rightMotorPWM, OUTPUT);
  pinMode(leftMotor1, OUTPUT);
  pinMode(leftMotor2, OUTPUT);
  pinMode(leftMotorPWM, OUTPUT);
  pinMode(motorPower, OUTPUT);
  
  delay(1000);
  manual_calibration(); 
}

int lastError = 0;
int  last_proportional = 0;
int integral = 0;


void loop()
{
  unsigned int sensors[5];
  int position = qtrrc.readLine(sensors);
  int error = position - 2000;
  
  Serial.print("position");
  Serial.println(position);
  Serial.print("error");
  Serial.println(error);

  int motorSpeed = KP * error + KD * (error - lastError);
  lastError = error;

  int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;
  int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;
  
  Serial.print("rightMotorSpeed");
  Serial.println(rightMotorSpeed);
  Serial.print("leftmotorspeed");
  Serial.println(leftMotorSpeed);
  
  delay(1000);
  
  {
  digitalWrite(motorPower, HIGH); // move forward with appropriate speeds
  digitalWrite(rightMotor1, HIGH);
  digitalWrite(rightMotor2, LOW);
  analogWrite(rightMotorPWM, rightMotorSpeed);
  digitalWrite(motorPower, HIGH);
  digitalWrite(leftMotor1, HIGH);
  digitalWrite(leftMotor2, LOW);
  analogWrite(leftMotorPWM, leftMotorSpeed);
  }
}




void manual_calibration() {

  int i;
  for (i = 0; i < 250; i++)  // the calibration will take a few seconds
  {
    qtrrc.calibrate(QTR_EMITTERS_ON);
    delay(20);
  }

  if (DEBUG) { // if true, generate sensor dats via serial output
    Serial.begin(9600);
    for (int i = 0; i < NUM_SENSORS; i++)
    {
      Serial.print(qtrrc.calibratedMinimumOn[i]);
      Serial.print(' ');
    }
    Serial.println();

    for (int i = 0; i < NUM_SENSORS; i++)
    {
      Serial.print(qtrrc.calibratedMaximumOn[i]);
      Serial.print(' ');
    }
    Serial.println();
    Serial.println();
  }
}


One thing is doubt on this code is the move forward function inside the main loop? is that fine? is that the task of the bot to move forward with appropriate speeds, or please suggest me a better technique. :slight_smile:

Also, please suggest me a sample code for automatic calibration which the 3pi does, first a left turn and a right turn, and zooooom. I understand that you make the bot turn right and left in calibration mode, but i am a bit confused, where to include this turning function? I don’t think my rough thought on this will work.

void setup() {
for (int i = 0; i < 400; i++)  // make the calibration take about 10 seconds
  {
    qtrrc.calibrate();       // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
 turn left();  // 90 degree left turn
delay(500);
turn right(); // 180 degree right turn
delay(1000);
turn left(); 90 degree left turn to final position
delay(500);
}
stop(); // end of calibration
}

I know, i got it wrong though :laughing:

And below is a serial print i get when the sensor is exactly above the black line, along with the radom motor speeds, and line positions and errors.

488 356 260 260 292 
2500 2500 2500 2500 2500 

position2247
error247
rmotightorspeed-1274
leftmotorspeed1294

position2186
error186
rmotightorspeed277
leftmotorspeed-257

position2274
error274
rmotightorspeed-484
leftmotorspeed504

position3788
error1788
rmotightorspeed-7917
leftmotorspeed7937

position4000
error2000
rmotightorspeed-1450
leftmotorspeed1470

position1682
error-318
rmotightorspeed11663
leftmotorspeed-11643

position2000
error0
rmotightorspeed-1580
leftmotorspeed1600

position2000
error0
rmotightorspeed10
leftmotorspeed10

as you can see, the line position is completely out of track, once the bot loses the track due to fast speed. Surprisingly, at the last serial reading, the error is 0, and motor speeds are at 10!! i didn’t notice it while experimenting, i was busy catching my mad line follower.

Is there a problem in the code? Or, am i fine to go and should i start tweaking the P and D values?

Last thing, i am using a 6v Ni-MH and boosted it to 9V using the pololu 9V boost regulator, it gets too hot, is it normal, it gets really hot? Just connected the Vin and gnd pins to +ve and -ve terminals of the battery, it gets really angry.

Thankyou for your time, and thankyou for such wonderful products.

Regards
Ashim

Hello, Ashim.

There is a lot of information in your post, but I found a few things that might help to start with. There are likely to be more things that need to be corrected, so I would suggest slowing down and addressing problems one at a time.

It sounds like you might be powering the QTR sensor array from 6V or 9V. If you are, it might damage (or have already damaged) the sensor array. It should be powered with 5V. Also, it sounded like you chose to use 5 sensor because of the line width, but as long as the line cannot get lost in between your sensors, it should not be a problem. You should be able to use all 8 sensors with a line width of 2.5cm.

How are you doing your manual calibration? You should be sweeping the QTR sensor over the line a couple of times, making sure that each sensor gets to detect the white and black surfaces. The sensors should stay roughly the distance from the surface that they will be when the robot is running. As for the automatic calibration, you should avoid using delays after the motor commands inside of an if statement. It is better to use non-blocking code for your motor commands and should look something like this:

for (int i = 0; i < 400; i++)
{
   if ( i  < 100 || i >= 300 )
     turn_right();
   else
     turn_left();
      
   qtrrc.calibrate();	
   delay(20);
}

You will most likely have to adjust either the timing or the speed of the motors so you do not over turn or under turn.

When working on a line follower, it is typical to limit your maximum speed until the robot is at least somewhat following a line. The minimum speed can typically be set to zero, since there should not be a need for the robot to go backwards in a timed race.

With regards to the regulator getting hot, they can heat up a little bit, but it might be drawing close to the maximum current output. If it draws more than the maximum output current of the regulator, the regulator could get damaged.

-Brandon

Thank you so much Brandon . I have powered the sensor array through the Vcc and GND pins on the arduino. How can I know if it’s damaged? It looks to be working fine.
And thank you for the rest of the info, will soon update on my progress :slight_smile:

Hello again,
I serial printed the raw values of all the 8 sensors, swept them across the line and they give pretty good values from 200 to 2500, the sensors look fine.

But still, my robot is always unstable, i spent a whole day changing the speeds, KP and KD values. I kept the KD value much higher than the KP value, and started from a value of 0 and moved on. Still, the program loop seems very unstable.

Please suggest what can i do to move forward.
Thank you
Ashim

Hello.

You generally want Kd to be much bigger than Kp. Another thing to make sure is that you have your signs right, as a sign error will produce unstable, positive feedback. You should start with just the Kp term get something that can follow a straight line decently at low speeds. Once you have that, try adding in Kd. There are a number of threads on this forum with tips on how to optimize a PID line-following algorithm (at least a few of which were written by me); you might want to try searching around for those if you continue to have trouble to see if that’s enough to help get you on track.

- Ben

Thankyou Ben,
Indeed, i read many of your posts, and got some idea on implementing PID into a line follower using the QTR-8RC sensors. I have tried for straight two days, i really think there is a problem in the code. Could you possibly say if i am on the right track?

#include <QTRSensors.h>

#define KP 0.5
#define KD 0
#define M1_MAX_SPEED 70
#define M2_MAX_SPEED 70
#define M1_DEFAULT_SPEED 50
#define M2_DEFAULT_SPEED 50
#define MIDDLE_SENSOR 4, 5
#define NUM_SENSORS  6     // number of sensors used
#define TIMEOUT       2500  // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN   2     // emitter is controlled by digital pin 2
#define DEBUG 1 // set to 1 if serial debug output needed

#define rightMotor1 3
#define rightMotor2 4
#define rightMotorPWM 5
#define leftMotor1 12
#define leftMotor2 13
#define leftMotorPWM 11
#define motorPower 8

QTRSensorsRC qtrrc((unsigned char[]) {  14, 15, 16, 17, 18, 19} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];

void setup()
{
  pinMode(rightMotor1, OUTPUT);
  pinMode(rightMotor2, OUTPUT);
  pinMode(rightMotorPWM, OUTPUT);
  pinMode(leftMotor1, OUTPUT);
  pinMode(leftMotor2, OUTPUT);
  pinMode(leftMotorPWM, OUTPUT);
  pinMode(motorPower, OUTPUT);
  
  
  manual_calibration(); 
}

int lastError = 0;
int  last_proportional = 0;
int integral = 0;


void loop()
{
  unsigned int sensors[6];
  int position = qtrrc.readLine(sensors);
  int error = position - 2500;

  int motorSpeed = KP * error + KD * (error - lastError);
  lastError = error;

  int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;
  int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;
  
    if (rightMotorSpeed > M1_MAX_SPEED ) rightMotorSpeed = M1_MAX_SPEED; // limit top speed
  if (leftMotorSpeed > M2_MAX_SPEED ) leftMotorSpeed = M2_MAX_SPEED; // limit top speed
  if (rightMotorSpeed < 0) rightMotorSpeed = 0; // keep motor above 0
  if (leftMotorSpeed < 0) leftMotorSpeed = 0; // keep motor speed above 0
  
   {
  digitalWrite(motorPower, HIGH);
  digitalWrite(rightMotor1, HIGH);
  digitalWrite(rightMotor2, LOW);
  analogWrite(rightMotorPWM, rightMotorSpeed);
  digitalWrite(motorPower, HIGH);
  digitalWrite(leftMotor1, HIGH);
  digitalWrite(leftMotor2, LOW);
  analogWrite(leftMotorPWM, leftMotorSpeed);
}
}

  
void manual_calibration() {

  int i;
  for (i = 0; i < 250; i++)  // the calibration will take a few seconds
  {
    qtrrc.calibrate(QTR_EMITTERS_ON);
    delay(20);
  }

  if (DEBUG) { // if true, generate sensor dats via serial output
    Serial.begin(9600);
    for (int i = 0; i < NUM_SENSORS; i++)
    {
      Serial.print(qtrrc.calibratedMinimumOn[i]);
      Serial.print(' ');
    }
    Serial.println();

    for (int i = 0; i < NUM_SENSORS; i++)
    {
      Serial.print(qtrrc.calibratedMaximumOn[i]);
      Serial.print(' ');
    }
    Serial.println();
    Serial.println();
  }
}





This is how my robot behaves. It follows the line for a split second, and when the error increases, it shoots off the line. Sounds like a tuning error, but could you take a look at the code?

Thanks,
Ashim

I don’t think it’s worth looking hard at your code until you have something more stable. Your current situation indicates that you have a sign error in your PID algorithm. What happens if you swap what you’re doing with your motors?

int rightMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;
int leftMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;

After calibration, you should just be able to hold the robot hovering over the line and watch as you slide it left and right to see whether the wheels turn in a way that would push it towards the line or away from it. If the wheels act to turn the robot away from the line, you have a sign error, and you should try to figure out where that’s coming from.

- Ben

Hello Ben,
Thank you for the quick reply.
I switched the signs in the equation . Voila!!!
I don’t know how, but looks like i have finally implemented PID!!!
What do you think was the problem, Ben?
Thanks!!! Will update my progress.
Ashim
Unbelievable :smiley:

As I said, you had a sign error in your PID algorithm. I’m very glad to hear it’s working now.

- Ben

Thanks Ben,
The bot is doing great now. While not that essential, it falters on a sharp turn ( >90 degrees), what alternative do you suggest. I was thinking of creating a small turning function for a while, just to handle the sharp turns. Or, is there another way? is it possible with further tuning, without creating the turn function, so that its speed won’t be compromised at any point in the track?

Also, having a little problem with automatic calibration. I used the following code:

for (int i = 0; i < 400; i++)
{
   if ( i  < 100 || i >= 300 )
     turn_right();
   else
     turn_left();
      
   qtrrc.calibrate();   
   delay(20); //  what is this delay for?
}
delay(5000); //  is this the right place to put a delay to stop my bot for 5s after calibration so that i can adjust its     //position?

Thanks :slight_smile:

I think you’re probably going to want to try to detect and handle sharp turns as a special case.

The 20 ms delay in the calibration code is intended to make the routine take a reasonable amount of time. You can add a 5 second delay at the end if you want to have time to move the robot, but you probably want to stop the motors first.

- Ben

Hey everyone,

I found this thread during some research for my line following project.

This code is really nice for autocalibrating.

for (int i = 0; i < 400; i++)
{
   if ( i  < 100 || i >= 300 )
     turn_right();
   else
     turn_left();
      
   qtrrc.calibrate();   
   delay(20); //  what is this delay for?
}
delay(5000); //  is this the right place to put a delay to stop my bot for 5s after calibration so that i can adjust its     //position?

How many degrees do I have to turn while calibration mode?

I got this function to rotate:

void RotateLeft (int duration, int pwm)
{
  digitalWrite(MotorLeftBrake, LOW);        // disengage brake motor left
  digitalWrite(MotorRightBrake, LOW);       // disengage brake motor right  
  digitalWrite(MotorLeftDirection, HIGH);    // forward motor left
  digitalWrite(MotorRightDirection, HIGH);  // forward motor right
  analogWrite(MotorLeftSpeed, pwm);         // speed motor left
  analogWrite(MotorRightSpeed, pwm);        // speed motor right

  delay(duration);                          // time moving forward

}

With the value of int duration I can decide how many degrees the robot will turn.

Do you have any recommendations?

Hi.

For the 3pi to use the QTR sensors to detect the line, each sensor must see both the line and the background of the line following course during calibration. The auto calibrating code in this thread is intended to be used while the 3pi is on top of the line of your line following course. It turns the 3pi to the left and right just as a way of moving each sensor over the line, so the exact angle of rotation that it goes through is not very important as long as each sensor sees both the line and the background.

The second piece of code that you posted will turn the robot, but the main point of the auto calibration method is to call the calibrate function regularly while turning the robot, so I recommend you just use the first piece of code in your post.

-Claire

A post was split to a new topic: Problems with QTR-8RC Sensor

A post was merged into an existing topic: Problems with QTR-8RC Sensor

A post was split to a new topic: Help with line follower using QTR8-RC

Shall i use this code for my gear motors ? I mean for automatic calibration?

Hello.

The examples in this thread might work for auto calibration of your robot; you should just try and see if it works for you.

Grant

Hello Ben,
I just want to ask that how to determine the setpoint value…i have taken position reading of 8 qtr sensor but it shows 3000 around cebtre and left or right-3500…please help me

It sounds like you are referring to the return value you are getting from the readLine function in our Arduino library. If you set it up properly, the function should be returning 3500 for the center and 0 or 7000 for each of the ends. You should make sure that your code correctly references the pins and number of sensors. If you continue to have problems, you should post your full code, and a video showing how you are calibrating and the readings you get as the robot moves over the line.

Grant