One manual interested me is the one about line tracking where they show a simple algorithm that works for easy paths with smooth curves. However, they explain that algorithm can not support paths that change abruptly and mentioned PID controller as a method to implement more sophisticated algorithm which can manage more difficult paths.
In this entry I want to explain my implementation of PID controller algorithm and compare it with the simple algorithm.
Line Tracking Module
First, we need to have a clear understanding of how and what is the data we can read from the line tracking module.
The line tracking module comes calibrated to detect black path under white or clear ground, and includes 3 sensors to detect if the path is at left, in the middle or at right.
Each one of those 3 sensors returns LOW signal when they detect black tape and HIGH signal otherwise. In the sketch provided by the manual they defined the readings as:
#define LT_R !digitalRead(10) #define LT_M !digitalRead(4) #define LT_L !digitalRead(2)
There is a negation operator (!) before each digital read, so the readings will be HIGH signal when they detect black tape and LOW signal otherwise.
Having that, we can describe all posible readings in following table.
| LT_L | LT_M | LT_R | Position |
|---|---|---|---|
| 0 | 1 | 0 | Aligned with path |
| 1 | 1 | 0 | Slightly turned right of path |
| 1 | 0 | 0 | Turned right of path |
| 0 | 1 | 1 | Slightly turned left of path |
| 0 | 0 | 1 | Turned left of path |
Simple Algorithm (no PID)
The algorithm included in Elegoo manual for line tracking uses a simple approach that we can see in the loop function.
1 2 3 4 5 6 7 8 9 10 11 12 13 | void loop() { if(LT_M){ forward(); } else if(LT_R) { right(); while(LT_R); } else if(LT_L) { left(); while(LT_L); } } |
Basically, the idea is to order the car to go forward, right or left according to the position of the car on the path. There are 3 cases:
- If middle sensor detects the path, car goes forward (lines 2 - 4).
- When the right sensor detects the path, means the car is heading to left, so the car needs to be turned to right (lines 5 - 8).
- When the left sensor detects the path, the car is heading to right, so the car is turned to left (lines 9 - 12).
The problem with this algorithm is when you use it on paths with deep curves or angles the car misses the path. In the moment when the car finds a deep curve or angle in the path it still will move according the last order from loop function, but a moment after it gets out of the path and all sensors don't detect the path, any of the if-statements in loop function won't be met.
A solution for this is to use a global variable as an indicator of which was the sensor that detected the path in previous execution of loop function and use that indicator as a criteria to order the car to move in next step. The loop function with this improvement is below.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 | void loop() { if(LT_M){ forward(); } else if(LT_R) { previousReading = READING_LT_R; right(); while(LT_R); } else if(LT_L) { previousReading = READING_LT_L; left(); while(LT_L); } else if (previousReading == READING_LT_R){ right(); } else if (previousReading == READING_LT_L){ left(); } } |
New instructions were inserted in lines 6, 11 to assign to previousReading a constant value to identify if the reading was made for LT_R or LT_L. Additionally, two else-if blocks were added in lines 14 - 18, so when 3 sensor readings (LT_M, LT_R, LT_L) don't detect the path, the previousReading is used to define in which direction the cars should move.
You can find the complete source code for this algorithm here.
PID Controller Based Algorithm
As I didn't have any knowledge or experience with PID Controller I was searching resources to learn the basics and I found some resources that helped me to understand. I recommend look at following links for this purpose:
- PID Control - A brief introduction: YouTube video.
- Simple Examples of PID Control: YouTube video.
- Control PID de Barra y Bola con Arduino: Nice application of PID to an Arduino project. Article is in Spanish but includes a YouTube video.
Apply PID
The PID will be applied in the middle of above workflow:
1 2 3 4 5 6 7 8 9 | float calculatePID(float pos, float setValue) { float error = pos - setValue; float derivative = error - previousError; integral += error; previousError = error; float pid = (error * kp) + (integral * ki) + (derivative * kd); return pid; } |
pos argument is some value that represents the position of the car relative to the path.
setValue argument is the target value where the position of the car would be perfect.
error (line 2) is the difference between the target position and the actual position.
derivative (line 3) is the difference between current error and error of previous execution.
integral (line 4) is the sum of all errors in each execution.
kp, ki and kd (line 6) are constants that we need to calibrate testing the algorithm with the car.
Calculate Position
Now, we need something in the middle of Line Tracking Module and Apply PID to convert the reading of 3 sensors (LT_L, LT_M, LT_R) into a position value.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 | 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; } } |
The idea behind this function is to return an integer value in range [-2, 2] to represent the position of the car relative to the path.
- (lines 6-8) Returns 0 if the position is completely aligned with the path.
- (lines 9-11) Returns -1 if the car is slightly towards left of the path.
- (lines 12-14) Returns -2 if the car is at left of the path.
- (lines 15-17) Returns 1 if the car is slightly towards right of the path.
- (lines 18-20) Returns 2 if the car is at right of the path.
- (lines 21-22) Returns previous calculated position if sensors didn't detect any path. This is a fallback in case the speed of the car make it out of the path.
Calculate Speed
Coming back to our workflow, we need to move the motors and the data available is PID output which is a float.
The intuition here is to use PID output to calculate the speed of right and left motors, so giving different speeds to right and left motors we can control the orientation of the car in the path. For example, if we give bigger speed to left motors, the car will move forward and slightly to its right. Another situation would be if we give a negative speed to left motors and positive speed to right motors, the car will turn to its left without going forward.
| Left Speed | Right Speed | Result |
|---|---|---|
| X | X | Forward |
| X | Bigger than X | Forward turning left |
| Bigger than X | X | Forward turning right |
| Negative | Positive | Turning left without forward |
| Positive | Negative | Turning right without forward |
1 2 3 4 5 6 7 8 9 10 11 12 13 | 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); } |
currentSpeedRight and currentSpeedLeft are global variables which keep the current speed and the delta argument will receive the output value from PID. Finally the right and left speeds are constrained to range [MIN_SPEED, MAX_SPEED] which are constants.
Moving Motors
The implementation of the workflow is almost complete, we only need to take the left and right speed and order the motors to move accordingly.
The layout of PINs for L298n module who is the driver of the four motors is as follows.
//define L298n module IO Pin #define ENA 5 #define ENB 6 #define IN1 7 #define IN2 8 #define IN3 9 #define IN4 11
From included manuals and examples, we have the following tables that help us to understand how to move the motors.
Left motor:
| ENA | IN1 | IN2 | Result |
|---|---|---|---|
| LOW | Not Applicable | Not Applicable | Motor is off |
| HIGH | LOW | LOW | Motor is stopped (brakes) |
| HIGH | HIGH | LOW | Motor is on and turning forwards |
| HIGH | LOW | HIGH | Motor is on and turning backwards |
| HIGH | HIGH | HIGH | Motor is stopped (brakes) |
Right motor:
| ENB | IN3 | IN4 | Result |
|---|---|---|---|
| LOW | Not Applicable | Not Applicable | Motor is off |
| HIGH | LOW | LOW | Motor is stopped (brakes) |
| HIGH | LOW | HIGH | Motor is on and turning forwards |
| HIGH | HIGH | LOW | Motor is on and turning backwards |
| HIGH | HIGH | HIGH | Motor is stopped (brakes) |
The direction of the car's movement:
| Left motor | Right motor | Result |
|---|---|---|
| stop(off) | stop(off) | Car is stopped |
| forward | forward | Car is running forwards |
| forward | backward | Car is turning right |
| backward | forward | Car is turning left |
| backward | backward | Car is running backwards |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 | 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); } |
As leftSpeed and rightSpeed can be negative values, the segment between lines 4 and 12 define if left motors should move forwards or backwards depending if speed is positive or negative; and similar control for right motors between lines 13 and 21.
Finally the actual speed values are the absolute values of the arguments, since motors support a speed between 0 and 255.
The code can be found here.
Finding Constant Values for PID
Once we have our PID implementation, we need to find the proper values for Kp, Ki and Kd constants. A good rule that helped me is:
Kd > Kp > Ki since derivative < error < integral
Derivative is expected to be the smallest value because is the difference between current error and previous error, integral is expected to be the greatest value because is the sum of all errors, and error is a middle value.
Then as Derivative is multiplied by Kd, we want to make Kd big so the result of (derivative * Kd) give us a significant value that can have relevance in whole PID calculation.
In same direction, error is multiplied by Kp and as error is a middle value, Kp would be a middle value as well.
Finally, we would a small value for Ki since it is multiplied by a big value (Ki * integral).
pid = (error * kp) + (integral * ki) + (derivative * kd);
Another recommendation is not to assign values to all 3 constants at first time. First, start with some value for Kp and make the other constants equal to zero. When you have a good value for Kp, you can go for Ki, and finally go for Kd.
Conclusions
I used two different paths and made a comparison between the simple algorithm presented in first section of this post and PID implementation.
Simple algorithm:
(+) Easy to implement and debug.
(+) Car runs at maximum speed.
(-) Car misses the path more frequently than PID.
PID implementation:
(+) Car stays in the path more than simple algorithm.
(-) Car doesn't use maximum speed.
(-) Hard to implement and debug.
In conclusion, if you want an accurate line tracking I suggest to go for PID, but if you don't mind the car misses the path sometimes, you can use a simple algorithm with a fallback routine to recover the path. Anyway, it is very good learning experience to implement PID and when you get the proper values for the constants is gratefully and fun.
Additional Observations
- When error and derivative are zero means the PID algorithm gave us a correct output and integral value should be reset to zero. I included this in my final implementation of PID.
- The formula applied for input signal in PID controller could be something more sophisticated than the used in my implementation (function getLineTrackerPosition()). However, I tried to do a formula like this one: ((leftReading * -10) + centerReading + (rightReading * 10)) / (leftReading + centerReading + rightReading), but I didn't get better results.
- I also tried to use an array to store the 5 previous calculated positions of the function getLineTrackerPosition() and use those values to calculate an average which I used it as input for PID. I didn't see significant improvements with this idea and made the code more complex, so I removed it.
- During calibration of PID constants, the PID output doesn't have an effect in speed of motors because of the velocity range that motor allows. A good understanding of correlation between the speed range and the PID output range is key.
- Printing values to Serial port and be able to view those values in monitor window of Arduino is very useful during implementation/debug process. You should leave the Arduino board connected to the USB port of your PC, but for that you'll need a longer cable than the one that is included in the kit.
- You could use Processing software to read data that Arduino board write to serial port and draw charts in Processing.
Resources
- Source code for simple algorithm in GitHub.
- Source code for PID in GitHub.
- Video of comparison between two algorithms.
- Processing: Software to read data from serial port and draw charts.
- PID Control - A brief introduction: YouTube video.
- Simple Examples of PID Control: YouTube video.
- Control PID de Barra y Bola con Arduino: Nice application of PID to an Arduino project. Article is in Spanish but includes a YouTube video.
