Let's Make Robots!

PID problems...

Hello guys,

 

I need to make a line follower for a competition...

I use:

Arduino

Ardumoto shield

30:1 reduction motors

QTRA sensors...

 

now following 3pi I got this code:

#include <QTRSensors.h>
#include <SoftwareSerial.h>
int s_left, s_right;
int i, pos, max_speed, last_prop;
int integral;
int speed_left = 3;  //PWM control for motor outputs 1 and 2 is on digital pin 3
int speed_right = 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()
{
  s_left=0;
  s_right=0;
  Serial.begin(9600);
  pinMode(speed_left, OUTPUT);
  pinMode(speed_right, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);
  analogWrite(speed_left, 0);  //PWD DOWN
  analogWrite(speed_right, 0); //PWM DOWN

  for (i = 0; i < 125; i++)    //5 second callibrate
  {
    qtra.calibrate();
  }
  Serial.print("PUSH!");
  Serial.println();
  while(digitalRead(2)==0);
  digitalWrite(12, LOW);  //Set motor direction, 1 low, 2 high
  digitalWrite(13, HIGH);
  analogWrite(speed_left, s_left);
  analogWrite(speed_right, s_right);

}

void loop()
{

  pos = qtra.readLine(sensors);
  int proportional = ((int)pos)-2000;
  int derivative = proportional - last_prop;
  integral += proportional;
  last_prop = proportional;
  int pid = proportional *(1/4) + integral/10000 + derivative*4;

  const int max_speed = 100;


  if (pid>max_speed)
  pid = max_speed;
  if (pid<-max_speed)
  pid= -max_speed;

  if(pid<0)
  {
    s_right = max_speed;
    s_left = (max_speed+pid);
  }
  else
  {
    s_right=((pid - max_speed)*-1);    //multiply by -1, not giving negative values to motors
    s_left=max_speed;
  }

 

  analogWrite(speed_left, s_left);
  analogWrite(speed_right, s_right);
}

void break_robot()
{
  analogWrite(speed_left, 0);
  analogWrite(speed_right, 0);
}

 

now the problem is that no matter what, my program won't run as I want... no matter what values I give, the returned PID will be always over 125... I mean values like 4574 or 354 or things like this...basically... it just makes the correction with static values

0 and 100

100 and 0

0 and 0

or 100 and 100

 

can you please help me out with optimizing the code?

 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

PID is almost as much art as science.  There are some really hairy equations that "should" work, but you have to know ALL the parameters of the ENTIRE system.  Not the controller, everything: motors, wheel friction, mass, turning radius, etc.  So, in practice it is often trial and error.  But, there is a systematic way of doing it.  The PID without a PHD article explains it.  The short answer is this.  First, P most likely should be LOWER than D.  D will make the response a lot faster very quickly, which will make the robot very jittery.  Start with P at the value that you find works best, leaving D and I at 0.  Once it is the best you can make it by adjusting only P, then start adding in D VERY slowly.  Not 4.0!  start with something like 0.1 and increase by 0.1 each time.  Remember that it CAN be negative.  I did a quick analysis and I don't think you need negative D, but I may have made a mistake.  In any event, you probably need a small D relative to P.  Whatever P works best, I suspect D should not be more than half that.  But like I said, try it in very small steps.  After D is adjusted properly, you can work on I.  The integral ONLY helps remove steady state error.  That means that the robot is stable, but has a constant error.  For instance, it tracks the line but stays constantly 1/2 inch to one side or the other.  That is the only kind of error the integral can fix.  Everything else has to be stable before you can even start on I.  And keep in mind, it may simply be that other factors of your robot simply won't allow it to be as stable as you like.  That is a fact of life.  Good luck and keep us posted.

 

now this one is explained :) thank you :D

still there are minor problems

for example if I keep kp higher than kd it's ok...

if I play with kp between 0.4 and 1.6 it's ok while the rest are 0...

 

I'm copy pasting from another forum where I posted:

 

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;

 

here is the video with the behaviour:

http://www.youtube.com/watch?v=AggF4ohxtAE

 

here is the final 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);
    }
  }
}

 

Glad to hear it.

well... it's working now :) :D

smooth :D

because it won't compile.  The line  diff = pos * P + integral * I + derivative * D;  will give you an error.  diff is an int and the result of the calculations will be  a float (because P,I, and D are floats) so now you have to cast that result to an int, like this:

diff = (int) (pos * P + integral * I + derivative * D);  That takes the float and converts it back to an int.

As for scaling, you take pos and subtract 2500.  That leaves a range of -2500 to 2500.  Then you divide that by 100 which gives a range of -25 to 25.  That's pretty small and you are losing resolution.  But then, for some reason I can't comprehend, you divide it again by 2, leaving you a range of only -12 to 12.  That might work, it's hard to say, but I can't see any reason to throw away that much of your data.  I would simply divide pos (after subtracting 2500 ) by 25, leaving a range of -100 to 100.  That's a good compromise that matches closely to your PWM values that are needed for output, as well as the range of numbers an int can handle.

Tuning the PID:  You make matters a lot worse by setting P to 1.0/7.0 which multiplied by the -12 to 12 range of pos only gives possible outputs of -1,0,1.  You also use 4.0 for D.  Like I said before, derivative is going to be troublesome and you should set D and I to 0.0, get the bot working as well as you can by tuning only P, then add I and D back in to tune it.  Read the article at the link I posted earlier about tuning the PID ( PID without a PHD ).  There are thick textbooks full of differential equations that describe tuning control systems, including PID.  It is NOT a simple subject.  That article is the best, simplest, clearest explanation I have ever seen.  That's why I posted it.  He can explain it a lot better than I can. You can scale the numbers before the PID calculations or after, either will work as long as you don't overflow values.  Look at the numbers like I did in this post and several others and figure out where they have to be scaled to prevent overflow, then make sure you scale them before that.  In short, pick a method and stick with it.  

So here is what I recommend at this point:

1.  put the cast into the PID calculation so it will compile

2.  change the scaling of pos to divide by 25 after subtracting 2500

3.  change P to 1.0, I and D to 0.0 until you get it working.  Adjust P ONLY to get any further scaling for proper outputs

4.  read the PID without PHD article, then begin tuning the PID  parameters ( I and D )

 

ok I made it that way...

 

#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()
{
  //  Serial.begin(9600);
  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()
{
  float P, I, D;
  P = 1.0/7.0;
  D = 4.0;
  I = 0.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;
    s_left = max_speed-diff;
    s_right = max_speed+diff;
  if(s_left>255)
  {
    s_left=255;
  }
  else if(s_left<0)
  {
    s_left=10;
  }

  if(s_right>255)
  {
    s_right=255;
  }
  else if(s_right<0)
  {
    s_right=10;
  }
  analogWrite(mleft, s_left);
  analogWrite(mright, s_right);
}

 

now the questions...

 

I am scaling the pid and the calculation right?

should I scale the final output, or the input for pid?

how to fine tune the pid?

If you go back to my very first post you will see that I told you to make the compiler know a number is floating point and to do floating point math, you MUST have a decimal point.  In P = 1/7;  The P is a float variable, but the math 1/7 is integers.  The compiler will do integer division on integers which still gives 1/7 = 0.  That's how it works.  No matter how much you argue that you are doing it right and it is the code's fault, not yours, it just simply will never give you anything but zero.  Sorry.  I can't change that.  Now, if you go back and read the first post I wrote, you will see that if you add a decimal point and a zero to the end of the numbers the compiler will know they are floating point values and do floating point math, like this:  P = 1.0 / 7.0

 

ok here is the final code:

 

#include <QTRSensors.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()
{
  float P, I, D;
  P = 1/7;
  D = 4;
  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;
  }
  else if(s_left<0)
  {
    s_left=10;
  }

  if(s_right>255)
  {
    s_right=255;
  }
  else if(s_right<0)
  {
    s_right=10;
  }
  analogWrite(mleft, s_left);
  analogWrite(mright, s_right);
}



still if I set these:

 

s_right = 0, it will run withouth a stop, no matter if serisal says it's 0;

that part works well... no problem with it...

 

I have trouble with this:

 

  if(diff>max_speed)
    diff=max_speed;
  if(diff<-max_speed)
    diff = -max_speed;

if I have a PID diff value of -8 it gives me 40

if it's 20 it makes it 40... I don't know why...

 

no matter if I use

{

}

or I don't use...

 

getting this: PID = prop*P + int*I + deriv*D;

where it can be made with prop*7/10 if I want to be multiplied by 0.7 but that's not what I asked... well... never mind...