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.

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

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

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.

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

 

the sensor reading returns me a value between 0 and 5000

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 = pos*P + integral*I + 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?

 

#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 = pos*P + integral*I + 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...

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...

 

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.

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;

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?

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

smooth :D