PID Line follower with tank treads - c

Question

I have made a (pretty bad) line follower.
Here is a sketch to roughly know the shape of the robot and location of the treads and sensors
[-] 0 0 [-] // 0 = color sensor
[-]------[-] // - = robot body
[-]------[-] // [-] = tank tread
[-] [-]
Here's what it does:
get Red, Green & Blue, make average of sensor 1 readings, do the same for 2
subtract to get value
this value will go through the PID part
steer with calculated steering
repeat (all of this is in a loop)
I use RGB and not reflected intensity (which is what is commonly used), because sometimes I need to detect if there's green color under the sensor (if there is, turn).
The real problem comes with the steering part. Unfortunately, it only accelerates a motor, meaning that in very tight turns we just lose the line.
Optimally, it should compensate a bit with the other motor (maybe going in the other direction?), but I am not sure how to calculate the speed of the motor, nor how to enforce this very strict line following policy.
Here is the code (I am also very grateful for any kind of tips on how to clean up the code! This is my first project in C :D ). I am not asking to read it all (it is pretty long), you could also just look at the steering function, and work your way back to rawFollowLine, this should hopefully shorten the code.
void rawFollowLine(int speed, float Kp, float Ki, float Kd){
_checkInit();
set_sensor_mode(sn_lx_color, "RGB-RAW");
set_sensor_mode(sn_rx_color, "RGB-RAW");
//printAllSensors();
int wasBlackCounter = 0;
int wasBlack = 0;
int lastBlack = 0;
for (int i = 0; i < 2000; i++)
{
if (isTerminating == 1)
{
killMotors(0);
break;
}
int greenCheck = rawGreenCheck(&wasBlack, &wasBlackCounter, &lastBlack);
if (wasBlack == 1){
wasBlackCounter++;
if (wasBlackCounter > 50){
wasBlackCounter = 0;
wasBlack = 0;
}
}
if (greenCheck == 1)
{
// lx is green
killMotors(1);
usleep(400 * 1000);
drive(200, 70);
waitIfMotorIsRunning();
killMotors(1);
pivotTurn(-90);
}
else if (greenCheck == 2)
{
// rx is green
killMotors(1);
usleep(400 * 1000);
drive(200, 70);
waitIfMotorIsRunning();
killMotors(1);
pivotTurn(90);
}
else if (greenCheck == 3)
{
// both rx and lx are green
killMotors(1);
turn(180);
}
else if (greenCheck == 5)
{
if(lastBlack == 2)
{
lastBlack = 0;
drive(100, -200);
//pivotTurn(50);
}
else if (lastBlack == 1)
{
lastBlack = 0;
drive(100, -200);
//pivotTurn(-50);
} else {
pidLineRaw(speed, Kp, Ki, Kd, &lastBlack);
}
}
else
{
pidLineRaw(speed, Kp, Ki, Kd, &lastBlack);
}
}
killMotors(1);
}
int rawGreenCheck(int *wasBlack, int *wasBlackCounter, int *lastBlack)
{
// Some documentation
// return nums:
// 3 = double green
// 2 = right green
// 1 = left green
// 0 = no green
int lx_red;
int lx_green;
int lx_blue;
int rx_red;
int rx_green;
int rx_blue;
get_sensor_value(0, sn_lx_color, &lx_red);
get_sensor_value(0, sn_rx_color, &rx_red);
get_sensor_value(1, sn_lx_color, &lx_green);
get_sensor_value(1, sn_rx_color, &rx_green);
get_sensor_value(2, sn_lx_color, &lx_blue);
get_sensor_value(2, sn_rx_color, &rx_blue);
//printf("rx_red %d\n", rx_red);
rx_red = (rx_red * rx_ratio_r);
rx_green = (rx_green * rx_ratio_g);
rx_blue = (rx_blue * rx_ratio_b);
//printf("rx_red (again) %d\n", rx_red);
if(
lx_red < 55 &&
lx_green > 90 &&
lx_blue < 55 &&
rx_red < 55 &&
rx_green > 90 &&
rx_blue < 55
)
{
// rx and lx see green
if (*wasBlack == 1)
{
// Apparently we crossed an intersection!
printf("Apparently we crossed an intersection!\n");
// We need to go straight.
*wasBlack = 0;
*wasBlackCounter = 0;
return 0;
}
else
{
return 3;
}
}
else if(lx_red < 55 && lx_green > 90 && lx_blue < 55)
{
// lx sees green
return 1;
}
else if(rx_red < 55 && rx_green > 90 && rx_blue < 55)
{
// rx sees green
return 2;
}
else if(rx_red < 50 && rx_green < 50 && rx_blue < 50 && lx_red < 50 && lx_green < 50 && lx_blue < 50)
{
// rx and lx see black
// this is needed if the intersection has the green tiles after the black line
printf("We are on the line? Is this an intersection?\n");
*wasBlack = 1;
return 0;
}
else if(lx_red < 55 && lx_green < 55 && lx_blue < 55)
{
// lx = right sees black
// this is needed if the intersection has the green tiles after the black line
//printf("We are on the line? Is this an intersection?\n");
killMotor(1, motor[R]);
rotateTillBlack(motor[L], sn_rx_color);
//printf("ASS2\n");
return 0;
}
else if(rx_red < 55 && rx_green < 55 && rx_blue < 55)
{
// rx = left sees black
killMotor(1, motor[L]);
rotateTillBlack(motor[R], sn_lx_color);
//printf("ASS1\n");
return 0;
}
//*lx_color_status = 0;
//*rx_color_status = 0;
*lastBlack = 0;
return 0;
}
void pidLineRaw(int speed, float Kp, float Ki, float Kd, int *lastBlack)
{
int red_lx_color;
int red_rx_color;
int green_lx_color;
int green_rx_color;
int blue_lx_color;
int blue_rx_color;
int lx_color;
int rx_color;
int last_error = 0;
int integral = 0;
int derivative = 0;
//float Kp = 0.1;
//float Ki = 0;
//float Kd = 0;
//set_sensor_mode(sn_lx_color, "COL-REFLECT");
//set_sensor_mode(sn_rx_color, "COL-REFLECT");
get_sensor_value(0, sn_lx_color, &red_lx_color);
get_sensor_value(0, sn_rx_color, &red_rx_color);
get_sensor_value(1, sn_lx_color, &green_lx_color);
get_sensor_value(1, sn_rx_color, &green_rx_color);
get_sensor_value(2, sn_lx_color, &blue_lx_color);
get_sensor_value(2, sn_rx_color, &blue_rx_color);
lx_color = (red_lx_color + green_lx_color+ blue_lx_color)/3;
rx_color = ( (red_rx_color*rx_ratio_r) + (green_rx_color*rx_ratio_g) + (blue_rx_color*rx_ratio_b))/3;
if(*lastBlack == 0)
{
int error = lx_color - rx_color;
integral = integral + error;
derivative = error - last_error;
last_error = error;
int steering_val = (error * Kp) + (integral * Ki) + (derivative * Kd);
// printf("error: %d\nsteering: %d\n",error, steering_val);
move_steering(-steering_val, speed, 1, 0);
} else if (*lastBlack == 1)
{
printf("lx_color_status\n");
move_steering(35, speed, 1, 0);
move_steering(-2, speed, 1, 0);
}
else if (*lastBlack == 2)
{
printf("rx_color_status\n");
move_steering(-35, speed, 1, 0);
move_steering(2, speed, 1, 0);
}
else
{
printf("HMMM: %d\n", *lastBlack);
exit(666);
}
}
static void _getSteeringSpeed(int speed, int *lx_speed, int *rx_speed, int steering)
{
if(steering > 100 || steering < -100)
{
printf("Yo wtf steering is %d\n", steering);
}
else
{
int speed_factor = (50 - abs(steering)) / 50;
*lx_speed = speed;
*rx_speed = speed;
if(steering >= 0)
{
*rx_speed = *rx_speed * speed_factor;
}
else
{
*lx_speed = *lx_speed * speed_factor;
}
}
}
Some parts are omitted, yes, they are not required to solve the problem.
I am also extremely sorry as there might be unused variables and such. I am working on refactoring the project, I'll update the post when I'm done.
So, summing everything up, I need to make sure that the steering part properly turns and follows the line. How do I do that? Is the code that I wrote even suitable? I'm guessing the steering itself might need some sort of feedback loop, to check if it's on the line?