Problem with PID line follower HELPP

Im using arduíno pro mini with QRT sensors and trying tô make a LINE follower code using PID, some translations might bê helpfull for understanding:
ÚltimoErro : LastError
And the car wont drive in a straight line even when the sensors read a error = 0
Any tips on why the code isnt working i would apreciate


// Portas driver motor
#define PININ1 2
#define PININ2 4
#define PININ3 5
#define PININ4 7
#define PINENA 3
#define PINENB 6

// Portas led rgb
#define PINLEDR 9
#define PINLEDG 11
#define PINLEDB 10

// Portas sensor QTR
#define S1 A0
#define S2 A1
#define S3 A2
#define S4 A3
#define S5 A4
#define S6 A5

// Valores de ajustes para o seguidor de linha MIF
#define TRESHOLD 700                       // Valor de referencia para cor da linha branca

#define RUNTIME 15500                      // Valor para executar o percurso 

void setup() {
  Serial.begin(9600);

}

void loop() {
  // TESTE 1°: leituta sensor
  //int s[6];
 //readSensors(true, s);
  // TESTE 2°: motor esquerda
 // motorOption('4',255,255);
   //TESTE 3°: motor direita
 // motorOption('6', 255, 255);
  // TESTE 4°: seguidor de linha
  followLineMEF();
  // TESTE 5°: teste led RGB
 // rgbControl(255,0,0,0);
 motorControl();
 
}



    int Kp = 1;
  int Kd = 100;
  int Ki = 1;
  float integral = 0;
  float derivada = 0;
  float ultimoErro = 0;
  int veloMotor = 0;
  int speedLeft, speedRight;  




void motorControl(void) {

  
  
  
  int erro = followLineMEF();
  integral = integral + erro;
  derivada = erro - ultimoErro;
  veloMotor = (Kp * erro) + (Ki * integral) + (Kd * derivada);
    


  
  // Função para controle do driver de motor
  // Definições das portas digitais
  pinMode(PININ1, OUTPUT);
  pinMode(PININ2, OUTPUT);
  pinMode(PININ3, OUTPUT);
  pinMode(PININ4, OUTPUT);
  pinMode(PINENA, OUTPUT);
  pinMode(PINENB, OUTPUT);



  // Ajustes motor da esquerda
  //frente
  if (erro == 0)  {
  speedLeft = 255;
  speedRight = 255;
  digitalWrite (PININ4,   HIGH);
  digitalWrite (PININ3, LOW);
  analogWrite (PINENA, speedLeft); 
  digitalWrite (PININ1, HIGH);
  digitalWrite (PININ2, LOW);
  analogWrite (PINENB, speedRight); 
  }
  
  //esquerda
  else if (erro <= 0)  {
    if (veloMotor > 255) {
      speedLeft = 255; 
      speedRight = 0;
    digitalWrite (PININ3, HIGH);
    digitalWrite (PININ4, LOW);
    analogWrite (PINENA, speedLeft); 
    digitalWrite (PININ1, LOW);
    digitalWrite (PININ2, HIGH);  
    analogWrite (PINENB, speedRight); 
    } else {
    speedLeft = 180 + veloMotor * -1;
    speedRight = 180 + veloMotor;
    digitalWrite (PININ3, HIGH);
    digitalWrite (PININ4, LOW);
    analogWrite (PINENA, speedLeft);
    digitalWrite (PININ1, LOW);
    digitalWrite (PININ2, HIGH);
    analogWrite (PINENB, speedRight);
    }
  }
 
  // Caso direita TA INDO ACHO
  else if (erro > 0) {
    if (veloMotor > 255) {
      speedLeft = 0;
      speedRight = 255;
      digitalWrite (PININ3, LOW);
      digitalWrite (PININ4, HIGH);
      analogWrite (PINENA, speedLeft);
      digitalWrite (PININ1, LOW);
      digitalWrite (PININ2, HIGH);
      analogWrite (PINENB, speedRight);
    }
    else {
    speedRight = 180 + veloMotor; 
    speedLeft = veloMotor * -1 + 255;
      digitalWrite (PININ3, HIGH);
      digitalWrite (PININ4, LOW);
      analogWrite (PINENA, speedLeft); 
      digitalWrite (PININ1, HIGH);
      digitalWrite (PININ2, LOW);
      
      analogWrite (PINENB, speedRight);
    }
  }
else {
  speedRight = 255;
  speedLeft = 255;

  digitalWrite (PININ4,   LOW);
  digitalWrite (PININ3, HIGH);
  analogWrite (PINENA, speedLeft); 
  digitalWrite (PININ1, LOW);
  digitalWrite (PININ2, HIGH);
  }
  ultimoErro = erro;
}

bool motorStop(long runtime, long currentTime) {
  // Função de parada do robô
  if (millis() >= (runtime + currentTime)) {
    
    int cont = 0;
    while (true) {
 
      cont++;
    }
    return false;
  }
  return true;
}




void readSensors(bool readSerial, int *sensors) {
  // Função para leitura dos sensores
  sensors[0] = analogRead(S1);
  sensors[1] = analogRead(S2);
  sensors[2] = analogRead(S3);
  sensors[3] = analogRead(S4);
  sensors[4] = analogRead(S5);
  sensors[5] = analogRead(S6);
  if (readSerial) {
    Serial.print(sensors[0]);
    Serial.print(' ');
    Serial.print(sensors[1]);
    Serial.print(' ');
    Serial.print(sensors[2]);
    Serial.print(' ');
    Serial.print(sensors[3]);
    Serial.print(' ');
    Serial.print(sensors[4]);
    Serial.print(' ');
    Serial.println(sensors[5]);
  }
}

int followLineMEF(void) {
  // Função para controle do seguidor de linha em modo de maquina de estado finita
  bool flag = true;
  long currentTime = millis();
  int erro;
  while (flag) {
    // Flag para verificar a parada
    flag = motorStop(RUNTIME, currentTime);

    // Leitura sensores
    int s[6];
    readSensors(true, s);

    // leitura do sensor (1 1 1 1 1 1)
    if (s[0] <= TRESHOLD && s[1] <= TRESHOLD && s[2] <= TRESHOLD && s[3] <= TRESHOLD && s[4] <= TRESHOLD && s[5] <= TRESHOLD) {
      //motorControl(0);
      erro = 0;
      // leitura do sensor (0 1 1 1 1 0)
    } else if ( s[0] >= TRESHOLD && s[1] <= TRESHOLD && s[2] <= TRESHOLD && s[3] <= TRESHOLD && s[4] <= TRESHOLD && s[5] >= TRESHOLD) {
     //motorControl(0);
     erro = 0;
      // leitura do sensor (0 0 1 1 0 0) //frente
    } else if ( s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] <= TRESHOLD && s[3] <= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
      //motorControl(0);
      erro = 0;

      // leitura do sensor (0 1 1 1 0 0)
    } else if (s[0] >= TRESHOLD && s[1] <= TRESHOLD && s[2] <= TRESHOLD && s[3] <= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
     // motorControl(1);
      erro = 1;
      // leitura do sensor (0 0 1 1 1 0)
    } else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] <= TRESHOLD && s[3] <= TRESHOLD && s[4] <= TRESHOLD && s[5] >= TRESHOLD ) {
     //motorControl(-1);
     erro = -1;

      // leitura do sensor (0 0 1 0 0 0)
    } else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] <= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
      //motorControl(2);
      erro = 2;
      // leitura do sensor (0 0 0 1 0 0)
    } else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] <= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD ) {
      //motorControl(-2);
      erro = -2;

      // leitura do sensor (0 1 1 0 0 0)
    } else if (s[0] >= TRESHOLD && s[1] <= TRESHOLD && s[2] <= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
    //motorControl(3);
    erro = 3;
      // leitura do sensor (0 0 0 1 1 0)
    } else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] <= TRESHOLD && s[4] <= TRESHOLD && s[5] >= TRESHOLD) {
    //motorControl(-3);
    erro = -3;

      // leitura do sensor (1 1 1 0 0 0)
    } else if (s[0] <= TRESHOLD && s[1] <= TRESHOLD && s[2] <= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
    //motorControl(4);
    erro = 4;
      // leit;ra do sensor (0 0 0 1 1 1)
    } else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] <= TRESHOLD && s[4] <= TRESHOLD && s[5] <= TRESHOLD) {
      //motorControl(-4);
    erro = -4;
      // leitra do sensor (0 1 0 0 0 0)
    } else if (s[0] >= TRESHOLD && s[1] <= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
     // motorControl(5);
      erro = 5;
      // leitura do sensor (0 0 0 0 1 0)
    } else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] <= TRESHOLD && s[5] >= TRESHOLD) {
     //motorControl(-5);
      erro = -5;
      // leitura do sensor (1 1 0 0 0 0)
    } else if (s[0] <= TRESHOLD && s[1] <= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
     //motorControl(6);
     erro = 6;
      // leitura do sensor (0 0 0 0 1 0)
    } else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] <= TRESHOLD && s[5] <= TRESHOLD) {
      //motorControl(-6);
     erro = -6; 
      // leitura do sensor (1 0 0 0 0 0)
    } else if (s[0] <= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
      //motorControl(7);
      erro = 7;
      // leitura do sensor (0 0 0 0 0 1)
    } else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] <= TRESHOLD) {
      //motorControl(-7);
    erro = -7;
    }
    return erro;
  }
}

Thanks for posting the code perfectly and showing us.
Sad You have a problem.

Take a look at this link for tips what to tell us: How to get the best out of this forum - Using Arduino / Project Guidance - Arduino Forum

1 Like

Why are these not in the setup function?

Compiles for me.

I used a pre-working line follower that didnt use PID and modified It, can try moving these to setup but they were working in the prévios project

But wont follow the line😔

Does it sense the line?

I just see this output from my non-existent hardware:

475 539 415 487 489 577
465 555 393 539 471 580
476 538 453 549 511 591
498 585 434 545 468 551
457 626 449 542 496 554

Why do you call followLineMEF(); and discard the results:

If it senses the line, maybe turn off the sensor printing and add this somewhere in motorControl:

Serial.print(erro);

I think i dont discard the followlinemef resutls, but now that you mencionei, i call the followlinemef at the motorcontrol function, so i should delete it form the loop?
Regarding the readings you sent i could not understand what you meant, It senses the line with the treshhold as when It read 700 it is the line

It is difficult to help since all we have is the code, and none of the hardware. So the best we can do is look at the code and guess what might be wrong.

Sloppy code confuses that -- since the loop() does call the followLineMEF() function, but then it doesn't seem to have a have a way to pass the results back, and then it is indeed called again within the other function where it actually reads the result.

I'd write your test-filled loop() like this:

void loop() {

switch( 1) {

 case 1: // TESTE 1°: leituta sensor
  //int s[6];
 //readSensors(true, s);
break;
  case 2:// TESTE 2°: motor esquerda
 // motorOption('4',255,255);
   break;
   case 3: //TESTE 3°: motor direita
 // motorOption('6', 255, 255);
   break;
  case 4: // TESTE 4°: seguidor de linha
  followLineMEF();
   break;
  case 5: // TESTE 5°: teste led RGB
 // rgbControl(255,0,0,0);
 motorControl();
   break;
  }
}

...in order use the compiler to ignore the bits that aren't under test, and to clearly separate the test cases without relying on commenting.

What is wrong with your robot? Is the the line detection? the PID? the motor driving? We can't ficure it out from the code alone.

1 Like

The hardware is 6 qrt sensors, and a arduíno pro mini,
If you need even more context regarding the hardware pls let me know as i am a noobie in these projects and dont know much.
and i dont know what is wrong with the robot either, as It doesnt follow the line, It doesnt even Go in a straight line when It is supposed to do, but comparing It to my colleagues It should be working, the changes to my colleagues is that their calculations of veloMotor, which theirs Just multiply the speedleft and speedright 0.8 times the veloMotor value according to the direction.
And thx for the answers

Instead of the senor readings, I'd try printing out erro, integral, derrivada, and veloMotor, to see if they seem as expected (and maybe try simulating erro to see if it steers as expected.)

void motorControl(void) {


  int erro = followLineMEF();
  //erro = -7*sin(millis()*2*PI/1000/30); // 30 sec sine
  integral = integral + erro;
  derivada = erro - ultimoErro;
  veloMotor = (Kp * erro) + (Ki * integral) + (Kd * derivada);
    
    Serial.print("erro:");
    Serial.print(erro);
    Serial.print(" i:");
    Serial.print(integral);
    Serial.print(" d:");
    Serial.print(derivada);
    Serial.print(" veloMotor:");
    Serial.print(veloMotor);
    Serial.println();
   ...

When I simulated erro as a sinusoidal time series, I saw veloMotor range far above 255 and below -255. For example:

erro:2 i:236.00 d:0.00 veloMotor:238
erro:2 i:238.00 d:0.00 veloMotor:240
erro:2 i:240.00 d:0.00 veloMotor:242
erro:2 i:242.00 d:0.00 veloMotor:244
erro:2 i:244.00 d:0.00 veloMotor:246
erro:2 i:246.00 d:0.00 veloMotor:248
erro:2 i:248.00 d:0.00 veloMotor:250
erro:2 i:250.00 d:0.00 veloMotor:252
erro:2 i:252.00 d:0.00 veloMotor:254
erro:2 i:254.00 d:0.00 veloMotor:256
erro:2 i:256.00 d:0.00 veloMotor:258
erro:2 i:258.00 d:0.00 veloMotor:260
erro:2 i:260.00 d:0.00 veloMotor:262
...
erro:-6 i:-872.00 d:0.00 veloMotor:-878
erro:-6 i:-878.00 d:0.00 veloMotor:-884
erro:-6 i:-884.00 d:0.00 veloMotor:-890
...
erro:0 i:1452.00 d:0.00 veloMotor:1452
erro:0 i:1452.00 d:0.00 veloMotor:1452
erro:0 i:1452.00 d:0.00 veloMotor:1452

which, with:

    speedLeft = 180 + veloMotor * -1;
    speedRight = 180 + veloMotor;

...could possibly go beyond the 0-255 range of an analogWrite into where you'd wrap and get discontinuities.

Also consider what happens to your code if veloMotor goes below -180, or even below.

I'd consider reducing veloMotor by a factor that keeps speedLeft and speedRight within feasible bounds. And maybe handling the case when if (veloMotor < -180)

Ill try to print the error and veloMotor to see If It is reading as intended, and now that you mencioned i didnt make a if to if the veloMotor goes below -180 and messes up with the speed, but ill still need to adjust the constants so that It stays semi-consistent.
Thx for the advice!

Good luck-- Printing out a PID's internal variables is good for diagnosing why it behaves as it does. PIDs can cause odd problems when they call for infeasible outputs. Integral windup - Wikipedia

1 Like

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.