HELP: PID Line follower not moving forward

Hello,

I recently purchased ELEGOO’s Smart Robot Car kit 3.0 for a line following project. The whole kit can be seen here ELEGOO UNO R3 Project Smart Robot Car Kit V 3.0 Plus – ELEGOO Official but in summary, the kit comes with a 3 channel IR sensor, L298N module, 4 DC Motors, a sensor shield and a MCU similar to Arduino UNO. The kit comes with a simple line following algorithm but I studied the control principle of PID and wanted to implement that instead.

The car follows a straight line just fine but as soon as it arrives near a bend (or if there is an error produced), the car starts oscillating left and right of the track without actually moving forward. It keeps doing that.
My guess was that the Kp is probably too large and thats why it was oscillating like that but I have tried reducing it and even tried adding in Kd to dampen the oscillations but the best I have gotten to is that the swing of the oscillations is reduced but the car still does not move forward and keeps oscillating left and right. P.S. I have only changed Kp and Kd values so far because I didnt think Ki is that necessary for line following so currently I’m only using a PD controller.

The code I used is as shown below.

//Line Tracking IO define
#define LT_R !digitalRead(10)
#define LT_M !digitalRead(4)
#define LT_L !digitalRead(2)

//motors
#define ENA 5 // controls left velocity
#define ENB 6 // controls right velocity
#define IN1 7 // left motor going forward
#define IN2 8 // left motor going backward
#define IN3 9 // right motor going backward
#define IN4 11 // right motor going forward

// logical (not actual) velocity defined to be as [MIN_SPEED, MAX_SPEED]
// actual velocity used in motors is [0, MAX_SPEED]
// motors support [0, 255] range but for PID we don't want 255 max speed
#define MAX_SPEED 150
#define MIN_SPEED -150
#define INITIAL_SPEED 100
#define LOGS_ON true //used for printing to serial monitor to help with tuning

int currentSpeedRight;
int currentSpeedLeft;
float previousError;
float integral;
int previousLineTrackerPos;

// Kd > Kp > Ki because derivative < error < integral
float kd = 130;
float kp = 20;
float ki = 0;

void setup() {
  setupLineTracker();
  setupMotors();
  initVariables();
}

void setupLineTracker() {
  Serial.begin(9600);
  pinMode(LT_R,INPUT);
  pinMode(LT_M,INPUT);
  pinMode(LT_L,INPUT);  
}

void setupMotors() {
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
}

void initVariables() {
  integral = 0;
  previousError = 0;
  currentSpeedRight = INITIAL_SPEED;
  currentSpeedLeft = INITIAL_SPEED;
}

void loop() {
  float pos = getLineTrackerPosition();
  float pid = calculatePID(pos, 0);
  calculateSpeed(pid);
  moveMotors(currentSpeedLeft, currentSpeedRight);
}

int getLineTrackerPosition() {
  int leftReading = LT_L;
  int centerReading = LT_M;
  int rightReading = LT_R;

  if (leftReading == LOW && centerReading == HIGH && rightReading == LOW) {
    previousLineTrackerPos = 0;
    return 0;
  } else if (leftReading == HIGH && centerReading == HIGH && rightReading == LOW) {
    previousLineTrackerPos = -1;
    return -1;
  } else if (leftReading == HIGH && centerReading == LOW && rightReading == LOW) {
    previousLineTrackerPos = -2;
    return -2;
  } else if (leftReading == LOW && centerReading == HIGH && rightReading == HIGH) {
    previousLineTrackerPos = 1;
    return 1;
  } else if (leftReading == LOW && centerReading == LOW && rightReading == HIGH) {
    previousLineTrackerPos = 2;
    return 2;
  } else {
    return previousLineTrackerPos;
  }
}

float calculatePID(float pos, float setValue) {
  float error = pos - setValue;
  float derivative = error - previousError;
  integral += error;
  previousError = error;

  if (error == 0 && derivative == 0) {
    //means output signal of PID converged to target signal
    //resets integral to zero and other control variables
    initVariables();
  }

  float pid = (error * kp) + (integral * ki) + (derivative * kd);

  if (LOGS_ON) {
    //Serial.println(error);
    //Serial.println(integral);
    char toPrint[43] = "";
    char value[10] = "";
    //error value to char array
    dtostrf(error, 10, 2, value);
    strcat(toPrint, value);
    strcat(toPrint, "|");
    dtostrf(integral, 10, 2, value);
    strcat(toPrint, value);
    strcat(toPrint, "|");
    dtostrf(derivative, 10, 2, value);
    strcat(toPrint, value);
    strcat(toPrint, "|");
    dtostrf(pid, 10, 2, value);
    strcat(toPrint, value);
    
    Serial.println(toPrint);
  }
  return pid;
}

void calculateSpeed(float delta) {
  int deltaInt = round(delta);

  //if car is at right (delta < 0), we need to turn left
  //increase right speed, decrease left speed
  //or if car is at left (delta > 0), we need to turn right
  //increase left speed, decrease right speed
  currentSpeedRight -= deltaInt;
  currentSpeedLeft += deltaInt;

  currentSpeedRight = constrain(currentSpeedRight, MIN_SPEED, MAX_SPEED);
  currentSpeedLeft = constrain(currentSpeedLeft, MIN_SPEED, MAX_SPEED);
}

void moveMotors(int leftSpeed, int rightSpeed) {
  int in1Signal, in2Signal, in3Signal, in4Signal;
  
  if (leftSpeed >= 0) {
    //left motors turning forwards
    in1Signal = HIGH;
    in2Signal = LOW;
  } else {
    //left motors turning backwards
    in1Signal = LOW;
    in2Signal = HIGH;
  }
  if (rightSpeed >= 0) {
    //right motors turning forwards
    in3Signal = LOW;
    in4Signal = HIGH;
  } else {
    //right motors turning backwards
    in3Signal = HIGH;
    in4Signal = LOW;
  }

  leftSpeed = abs(leftSpeed);
  rightSpeed = abs(rightSpeed);
  analogWrite(ENA, leftSpeed);
  analogWrite(ENB, rightSpeed);
  digitalWrite(IN1, in1Signal);
  digitalWrite(IN2, in2Signal);
  digitalWrite(IN3, in3Signal);
  digitalWrite(IN4, in4Signal);
}

I thought the problem was with tuning but now I’m not sure if its the tuning or if there’s a flaw in the algorithm?
I also attached a text file of the serial monitor output in case that helps. The values are printed in the form Time->Error l Integral l Derivative l PID value
Any help would be greatly appreciated!

pid_serial_output.txt (21.3 KB)

The derivative and integral terms are not scaled properly, because you are not keeping track of time. PID works best if the loop update interval is constant, and at least 10 times faster than the vehicle response time.

However, with just three possible values for the error term (ignoring sign), it is hard to see how proportional control can be very effective. You would be better off with a sensor array like this one.

Thanks for using code tags on your first post! Karma++

jremington:
The derivative and integral terms are not scaled properly, because you are not keeping track of time. PID works best if the loop update interval is constant, and at least 10 times faster than the vehicle response time.

However, with just three possible values for the error term (ignoring sign), it is hard to see how proportional control can be very effective. You would be better off with a sensor array like this one.

Thanks for using code tags on your first post! Karma++

Thank you very much for your reply!
It makes sense but my only problem with the QTR sensor is that it might slightly be too big for the track I am using. In particular, the track has U-shaped narrow bends like the one I attached to this reply.
If the car is coming from the top side of the picture, do you think the sensors on the right side of the QTR module might end up interfering with the turn?

Track U Turn.png

Track U Turn.png

The photo does not even hint at the dimensions of the track.

No need to copy my post. It isn’t going anywhere.

Apologies for that.

The width of the black line is 25mm and the maximum gap between the 2 black lines is 10mm as shown in the picture attached.

Fix the scaling of D and I first, then consider other options.

That track arrangement creates an interesting programming problem for any sensor.

With the QTR-8RC you could ignore a “black” reading from an outer sensor, if there were a “white” gap between it and the center sensors.

But for a first try, my approach would be to calculate a weighted center position from the entire sensor array, which might be good enough for PID line following even for that sharp curve.

For example, suppose the array so has the outputs (0 for white, 1 for black) of the eight sensors, 0 to 7, located at xs={-3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5} in arbitrary units, measured from the center of the sensor array (the actual sensor spacing is 0.375 inches or 9.5 mm).

Code to calculate the weighted line center position, relative to the sensor array center, could look something like this:

float xs[]={-3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5}; //sensor x positions
float center=0.0, sum_weights=0.0;
for (int i=0; i<8; i++) {
sum_weights += so[i]; //weight on each position
center += so[i]*xs[i]; //weighted sum
}
center /= sum_weights; //normalize by sum of weights

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