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
#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;
}