PID problems...

Community General Discussions
1

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?

 

2

I am unsure about

QTRSensorsAnalog qtra((unsigned char[]) { 0, 1, 2, 3, 4, 5}, 6);

I just don’t follow the logic of that line.

3

me neither… although this

me neither… although this is for being there… via pololu/sparkfun library…

 

the sensor reading returns me a value between 0 and 5000

4

Looking at the library

Looking at the library code:
QTRSensorsAnalog(unsigned char* pins, unsigned char numSensors, unsigned char numSamplesPerSensor = 4, unsigned char emitterPin = 255);

Seems like they were lazy to do a sizeof and you must specify the number of sensors.

5

void loop(){ int integral,

void loop()
{
  int integral, last_proportional;
  const int max_speed = 40;
  pos = qtra.readLine(sensors);
  pos = pos-2500;
  pos = pos/100;
  Serial.print(pos);
  Serial.print("\t");
  int derivative = pos - last_proportional;
  integral = integral+pos;
  last_proportional = pos;
  int diff = posP + integralI + derivative * D;
  Serial.print(diff);
  Serial.print("\t");
  if(diff>max_speed)
  diff=max_speed;
  if(diff<-max_speed)
  diff = -max_speed;
  if(diff<0)
  {
    s_left=max_speed-diff;
    s_right=max_speed+(diff/2);
  }
  else
  {
    s_left = max_speed-(diff/2);
    s_right = max_speed+diff;
  }
  Serial.print(s_left);
  Serial.print("\t");
  Serial.print(s_right);
  Serial.print("\t");
  Serial.println();
  analogWrite(speed_left, s_left);
  analogWrite(speed_right, s_right);
}

 

as you see I have this part:

  pos = qtra.readLine(sensors);
  pos = pos-2500;
  pos = pos/100;

should I split the pos (proportional) so I will get values much lower or leave without the division?

 

6

#include

#include <QTRSensors.h>
#include <SoftwareSerial.h>
const int max_speed = 40;
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()
{ //between 0-125
  Serial.begin(9600);
  pinMode(mleft, OUTPUT);
  pinMode(mright, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);

  Serial.print(“Ready to calib”);
  Serial.println();
  while(digitalRead(2)==0);
  Serial.print(“Calibrating!”);
  Serial.println();
  int i;
  for (i = 0; i < 80; i++)
  {
    qtra.calibrate();
  }
  Serial.print(“Calibrate OK! Waiting command”);
  Serial.println();
  while(digitalRead(2)==0);
  digitalWrite(12, LOW);
  digitalWrite(13, HIGH);
  //  Serial.end();
  analogWrite(mleft, max_speed);
  analogWrite(mright, max_speed);

}

void loop()
{
  int P, I, D;
  P = 1/10;
  D = 1;
  I = 0;
  int integral, last_proportional, diff, derivative;
  int pos = qtra.readLine(sensors);
  pos = pos-2500;
  pos = pos/100;
  Serial.print(pos);
  Serial.print("\t");
  derivative = pos - last_proportional;
  integral = integral+pos;
  last_proportional = pos;
  diff = posP + integralI + derivative * D;
  Serial.print(diff);
  Serial.print("\t");
  if(diff>max_speed)
  {
    diff=max_speed;
  }
  if(diff<-max_speed)
  {
    diff = -max_speed;
  }
  Serial.print(diff);
  Serial.print("\t");
  if(diff<0)
  {
    s_left=max_speed-diff;
    s_right=max_speed;
  }
  else
  {
    s_left = max_speed;
    s_right = max_speed+diff;
  }
  analogWrite(mleft, s_left);
  analogWrite(mright, s_right);
  Serial.print(s_left);
  Serial.print("\t");
  Serial.print(s_right);
  Serial.print("\t");
  Serial.println();
  delay(200);
}

 

here is the final code, although I don’t know why, but my “diff” variable, becomes 40 all the time…

 

I changed max_speed to 40 to configure PID… but well… it just doesn’t work…

7

that part works well… no

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 = propP + intI + derivD;

where it can be made with prop7/10 if I want to be multiplied by 0.7 but that’s not what I asked… well… never mind…

 

8

Do you know about debugging programs?

If you would use Serial.println(var); , you would see that bdk6 is telling you the truth. You are declaring variables as integers and then doing math with them. Integer math will only ever give you integers for an answer. 1 / 10 = 0.1 != an integer. Until you understand that you can not get floating point numbers from integer math, you will forever be stuck here.

I know I am coming off totally rude. I’m sorry. I don’t know how to explain it better via just text.

9

ok here is the final

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 = posP + integralI + 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;

10

ok I made it that

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 = posP + integralI + 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?

11

well… it’s working now :slight_smile:

well… it’s working now :slight_smile: :smiley:

smooth :smiley:

12

still there are minor

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… :frowning:

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§;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) (posP + integralI + derivativeD);
  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 = P10;
  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 = D10;
  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 = I10;
  EEPROM.write(3, I);
  P = P/10;
  D = D/10;
  I = I/10;
  Serial.println();
  Serial.print("kP: “);
  Serial.print§;
  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);
    }
  }
}