Line Follower PID huge problems

mihalyn90 · May 18, 2013, 7:51pm

Hello

So after ordering compontents everything and assembled the whole thing
here is the code:

#include <QTRSensors.h>
#include <SoftwareSerial.h>
const int max_speed = 50;
int s_left, s_right;
int mleft = 3;  //PWM control for motor outputs 1 and 2 is on digital pin 3
int mright = 11;  //PWM control for motor outputs 3 and 4 is on digital pin 11
QTRSensorsAnalog qtra((unsigned char[]) {
  0, 1, 2, 3, 4, 5}
, 6);
unsigned int sensors[6];


void setup()
{
  pinMode(mleft, OUTPUT);
  pinMode(mright, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);

  while(digitalRead(2)==0);
  int i;
  for (i = 0; i < 80; i++)
  {
    qtra.calibrate();
  }
  while(digitalRead(2)==0);
  digitalWrite(12, LOW);
  digitalWrite(13, HIGH);
  analogWrite(mleft, max_speed);
  analogWrite(mright, max_speed);

}

void loop()
{
  int P, I, D;
  P = 6;
  D = 0;
  I = 0;
  int integral, last_proportional, diff, derivative;
  int pos = qtra.readLine(sensors);
  pos = pos-2500;
  pos = pos/100;
  pos = pos/2;
  derivative = pos - last_proportional;
  integral = integral+pos;
  last_proportional = pos;
  diff = pos*P + integral*I + derivative*D;
  if(diff<0)
  {
    s_left=max_speed-diff;
    s_right=max_speed+diff;
  }
  else
  {
    s_left = max_speed-diff;
    s_right = max_speed+diff;
  }
  if(s_left>255)
  {
    s_left=255;
  }
  if(s_left<0)
  {
    s_left=0;
  }
  if(s_right>255)
  {
    s_right=255;
  }
  if(s_right<0)
  {
    s_right=0;
  }
}

now what is strange…
if(s_left<0)
s_left=0… doesn’t work when I add “D” 1 or over
same thing with s_right<0

if I keep Kp>1… there is no problem
if I add kD to be greater than 1 everything goes worng… any ideea?

Jim_Remington · May 18, 2013, 11:50pm

Why not explain what you mean by “doesn’t work” and “everything goes wrong”?

mihalyn90 · May 19, 2013, 9:18am

Opps sorry… there were around 20 hours as I didn’t slept…

the idea is that sometimes, if the power_difference that needs to be put to the motors and is calculated via pid has errors.

after adding or substracting it from the motor power I should have a value no more or less than 0-255…

now I here is the interesting part:

if(s_left>255)
s_left = 255;
if(s_right<0)
s_right=0;

this part of the code, where it should limit the low and high value it simply doesn’t work… sometimes it give -x or even 300… no matter if it’s there or no…

I tried making a scaler last time

calculate pid
return value to "diff"
const int max_speed 40;
if(diff<-max_speed)
diff = -max_speed)
if(diff>max_speed)
diff=max_speed;

now, no matter what, if my pid gave a value of 5 to diff… this part of code made it 40. if I got a value of -10, this part of code, made it 40. if the value was 500, the code still made it 40; and I don’t know why

Jim_Remington · May 19, 2013, 3:45pm

The code looks OK. My guess is that either the compiler is making errors or more likely, memory is being corrupted by out-of-bounds array access.

I would put in lots of print statements to see what quantities are actually stored in the variables. For example, do you print out the value of “diff” just before and just after you go through the “if” statements?

By the way, you shouldn’t need the integral term in line following.

mihalyn90 · May 20, 2013, 12:06am

as it’s fine up the value at the end after more and more laps… I think it’s ok…
now it depends of how much will it help…

I remade the code:
I store the PID values in eeprom just in case so I don’t have to flash again and again…
when I found all of them… I simply flash it up without the Serial communication part.

now according to my excel of calculation…
with P = 0.5
and D = 2
my diff, varies between 100002.5 and -12.5;

with P = 1.6
and D = 0
my diff varies between: -40 and 40 which give my robot a nice follow as in the video here:

this is the code:

#include <QTRSensors.h>
#include <SoftwareSerial.h>
#include <EEPROM.h>

float P, I, D;
const int max_speed = 255;
int s_left, s_right;
int mleft = 3;  //PWM control for motor outputs 1 and 2 is on digital pin 3
int mright = 11;  //PWM control for motor outputs 3 and 4 is on digital pin 11
QTRSensorsAnalog qtra((unsigned char[]) {
  0, 1, 2, 3, 4, 5}
, 6);
unsigned int sensors[6];


void setup()
{
  pinMode(mleft, OUTPUT);
  pinMode(mright, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);
  pinMode(2, OUTPUT);
  Serial.begin(9600);
  P = EEPROM.read(1);
  D = EEPROM.read(2);
  I = EEPROM.read(3);
  P = P/10;
  D = D/10;
  I = I/10;
  Serial.print("Stored values for PID are:");Serial.println();
  Serial.print("kP: ");Serial.print(P);Serial.print(" kD: ");Serial.print(D);Serial.print(" kI: ");Serial.print(I);
  Serial.println();

  pid_setup();
  calib_message();
  int i;
  for (i = 0; i < 80; i++)
  {
    if(i < 20 || i >= 60)
      go(120,-120);
    else
      go(-120,120);
    qtra.calibrate();
    go(0,0);
  }
  Serial.print("Calibration ok!");
  Serial.println();
  Serial.print("Press button to START");
  Serial.println();
  Serial.end();
  pinMode(2, INPUT);
  while(digitalRead(2)==0);
  digitalWrite(12, LOW);
  digitalWrite(13, HIGH);
  analogWrite(mleft, max_speed);
  analogWrite(mright, max_speed);

}

void loop()
{
  int integral, last_proportional, diff, derivative;
  int pos = qtra.readLine(sensors);
  pos = pos-2500;
  pos = pos/10;
  derivative = pos - last_proportional;
  integral = integral+pos;
  last_proportional = pos;
  diff =(int) (pos*P + integral*I + derivative*D);
  s_left = max_speed - diff;
  s_right = max_speed + diff;

  go(s_left, s_right);
}

void pid_setup()
{
  Serial.print("kP = ");
  while(Serial.available()==0);
  P = Serial.parseFloat();
  Serial.print(P);
  P = P*10;
  EEPROM.write(1,P);
  Serial.println();
  Serial.end();

  Serial.begin(9600);
  Serial.print("kD = ");
  while(Serial.available()==0);
  D = Serial.parseFloat();
  Serial.print(D);
  D = D*10;
  EEPROM.write(2, D);
 
  Serial.println();
  Serial.end();

  Serial.begin(9600);
  Serial.print("kI = ");
  while(Serial.available()==0);
  I = Serial.parseFloat();
  Serial.print(I);
  I = I*10;
  EEPROM.write(3, I);
  P = P/10;
  D = D/10;
  I = I/10;
  Serial.println();
  Serial.print("kP: ");
  Serial.print(P);
  Serial.print(" kD: ");
  Serial.print(D);
  Serial.print(" kI: ");
  Serial.print(I);
  Serial.println();
}
void calib_message()
{
  Serial.print("Calibration starts in... 3... ");
  delay(1000);
  Serial.print("2... ");
  delay(1000);
  Serial.print("1... ");
  Serial.println();
  Serial.print("Calibration started");
  Serial.println();
  delay(200);
}

void go(int speedLeft, int speedRight) 
{
  if (speedLeft > 0) 
  {
    digitalWrite(12, LOW);
    if (speedLeft > 254)
    {
      analogWrite(mleft, 254);
    }
    else
    {
      analogWrite(mleft, speedLeft);
    }
  }
  else 
  {
    digitalWrite(12, HIGH);
    if(speedLeft < -254)
    {
      analogWrite(mleft, 254);
    }
    else
    {
      analogWrite(mleft, -speedLeft);
    }
  }


  if (speedRight > 0)
  {
    digitalWrite(13, HIGH);
    if(speedRight > 254)
    {
      analogWrite(mright, 254);
    }
    else
    {
      analogWrite(mright, speedRight);
    }
  }
  else 
  {
    digitalWrite(13, LOW);
    if(speedRight < -254)
    {
      analogWrite(mright, 254);
    }
    else
    {
      analogWrite(mright, -speedRight);
    }
  }
}

the questions:
should i scale lower the final diff?
should I increase P, and leave D and I = 0?

As I observed… with D = 0, and P = 1.6 my robot wobbles a bit like in the video…
if I increase D… my robot just breaks more and more often hardly following the line…
following this reaction Kp is much more higher than Kd… that gives me a nice following of the line…
although Kp should be lower than Kd… I don’t know what to do…

now I’m stuck with a wobby mice looking robot which doesn’t loose the line… but isn’t smooth…

meanwhile I mean another video was made.
when in the video the robot stops… I write on new values for P, I, D;

here are the values used in chronological order:
P = 1.6, D = 0, I = 0;
P =0.1, D = 4, I = 0;
P = 0.2, D = 4, I = 0;
P = 0.5, D = 4, I =0;

mihalyn90 · May 21, 2013, 5:36pm

back again

here is the thing… I don’t really know what should I do…

as I observed my code here is the thing:

if I give higher values than 2 or 3 to P and D, my robot motors will be only switch between -254 and 254…

should I scale down the output of the pid, or should I scale lower the reading from the sensors?
or both?

Jim_Remington · May 22, 2013, 1:17am

There are well understood procedures for correctly adjusting a PID controller and if you google something like “pid tuning tutorial” you will find a number of examples. As I mentioned, you don’t need the “I” term, which simplifies the problem. You almost always want to find a suitable value of Kp first, then start to adjust Kd. After you get close, there may be some back-and-forth to optimize Kp and Kd.

It is easiest if you can print out numbers to see how the motors are being told to respond to different situations. They have to make sense to you before you can properly choose appropriate values for the tuning constants. In particular, the sign of a correction term has to be correct (it is easy to get this wrong).

mihalyn90 · May 22, 2013, 7:46am

I remember driving my motors insane back and forth and turning right instead of left etc. Now everythings seems to work

void loop()
{
  int last_proportional, diff, derivative;
  int pos = qtra.readLine(sensors);
  pos = pos-2500;
  pos = pos/100;
  derivative = pos - last_proportional;
  last_proportional = pos;
  diff = pos*P + derivative*D;
  s_left = max_speed - diff;
  s_right = max_speed + diff;
  go(s_left, s_right);
}

as you see, I scaled down the pos, which is the proportional value… now everything looks good, I can play kP up to 15 from 0. When I didn’t scaled the proportional part I could only used 1.5 which was rather much, and D only 0.1 etc.
Now I have smoothness with kP = 3, kD = 15.
Although I am decreasing the speed of a motor and reversing the second motor, it makes turns much more aggressive