Go Down

Topic: [Help] How to calculate cross track error in PID algorithm (Read 2212 times) previous topic - next topic

logicalway

Hi i'm currently making a line follower racing car. It looks like in this video. This car senses the white line using an array of 8 infrared sensors. Everyone in my school programs the car using a giant switch case like in this document so they have to hardcode all the steering angel of the car at each case. I want to do it differently so I decided to use PID algorithm on this problem but can't get it to work properly. Because the racing track has three white lines instead of one. I can't come up with a right way to calculate cross track error to pass to my pid function. Here is my current implementation of the function. It doesn't work really well and the car runs out of the track at high speed and very shaky. Hope anyone can help me. Sorry for my bad English and if I can't describe the promblem clear enough. Thanks for helping me

Code: [Select]
#define mask3_0 0b11100000
#define mask0_3 0b00000111

/*
8 sensors
*   *   *   *   *   *   *   *
-3  -2  -1          1   2   3

*/

int16_t last_cte = 0;

int16_t calc_cte(uint8_t sensor_val) { //calculate cross track error
uint8_t t = 0;
int16_t ret;

sensor_val &= 0b01111110; //mask out two outermost bit
if (sensor_val == 0b00011000 || sensor_val == 0b00010000 || sensor_val == 0b00001000) { //if the car is in the middle of the track return 0
return 0;
}
if ( (sensor_val & mask3_0) != 0) t += 1;
if ( (sensor_val & mask0_3) != 0) t += 2;

if (t == 1) { //right
sensor_val >>= 5;

for(uint8_t i = 0; i < 3; i++) {
if ( (sensor_val & (0b100 >> i)) != 0 ) {
ret = (3*m - m*i);
last_cte = ret; //using last cte in case the car is in the middle of two white lines
return ret;
}
}
}

if (t == 2) { //left
for(uint8_t i = 0; i < 3; i++) {
if ( (sensor_val & (1 << i)) != 0) {
ret = -(3*m - m*i);
last_cte = ret;
return ret;
}
}
}
if (last_cte > 0) return 400;
else if (last_cte < 0) return -400;
}



jremington


logicalway

i got the cross track error definition from here https://www.youtube.com/watch?v=-8w0prceask. It's the value where I use to calculate the proportional term. p_term = (cross_track_error - setpoint)*p_constant. setpoint is the value where we want to be.

JasonK

based on your referenced video and code segment: something like this

... and so on ...
0b00110000 -> -2
0b00010000 -> -1
0b00011000 ->  0
0b00001000 ->  1
0b00001100 ->  2
... and so on ...

this assumes that your line will over lap 1 or 2 sensors at any time, this will give you 15 values ranging from -7 to 7.

logicalway

Do think pid is possible for this kind of problem ? No matter how I try I can't get pid to work well enough. It's unstable and shaky.

JasonK

PID is possible for this kind of problem.

Things to account for:
with discrete error values, you will need to do some sort of time averaging for your derivative term, or it will be overly noisy.

You will also need to tune the PID.

Quote
so they have to hardcode all the steering angel of the car at each case.
if the values are linear, this is just a the P part of a PID controller.

Go Up