Hello, everyone! I am trying to make a line following robot using the Pololu QTR-8RC sensor. The robot follows straight lines like a charm but struggles to make it through 90 degree turns.
I wrote some code of my own that checks the value of all the sensors against a pre-set threshold and then decides which direction to turn in. This is a breakout code that runs when all the sensors are on a white surface i.e. the robot has just missed the turn. The robot stops, reverses and rotates in the direction where the black line last was. This code somewhat works but not really well.
Using a Nano, L298N motor driver, 12V battery pack, EDVON kit motors and tires.
Here's my code:
#include <QTRSensors.h>
//variables for PID controller
#define Kp 0.07
#define Kd 0.235
int turn_reversing_power_duration = 170; //to slightly reverse the robot on a turn for better navigation
int reverse_speed = 130; //speed with which the car reverses
bool reverse_enable = true;
int rotation_duration = 90; //time to rotate the car to reallign itself on the line after a hard turn
int rotation_speed = 120;
int threshold = 1000; //to detect when the robot is off the black line
int test=1; //to test the threshold setting
//float speed_reduction_factor = 0.9;
#define rightMaxSpeed 120
#define leftMaxSpeed 120
#define rightBaseSpeed 110
#define leftBaseSpeed 110
int turn_counter=0; //to slow down the car if it encounters consecutive turns
int turn_off_counter=0;
#define NUM_SENSORS 8
#define TIMEOUT 2500
#define EMITTER_PIN 7
#define rightMotor1 12
#define rightMotor2 11
#define rightMotorPWM 6
#define leftMotor1 10
#define leftMotor2 9
#define leftMotorPWM 5
int SumLeft = 0;
int SumRight = 0;
int SumDifference = 0;
int Last = 0;
int LastLeftval;
int LastRightval;
QTRSensorsRC qtrrc((unsigned char[]) {A7, A6, A5, A4, A3, A2, A1, A0}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
void setup() {
Serial.begin(9600);
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);
pinMode(rightMotorPWM, OUTPUT);
pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(leftMotorPWM, OUTPUT);
int i;
for (int i = 0; i < 100; i++) {
qtrrc.calibrate();
delay(20);
}
wait();
delay(2000);
}
int lastError = 0;
void loop() {
unsigned int sensors[8];
int position = qtrrc.readLine(sensors, QTR_EMITTERS_ON, 1);
Serial.print("Line position: ");
Serial.println(position);
int error = position - 3500;
SumRight = (sensors[0] + sensors[1] + sensors[2] + sensors[3]);
SumLeft = (sensors[4] + sensors[5] + sensors[6] + sensors[7]);
SumDifference = (SumLeft - SumRight);
int motorSpeed = Kp * error + Kd * (error - lastError);
lastError = error;
int rightMotorSpeed = rightBaseSpeed + motorSpeed;
int leftMotorSpeed = leftBaseSpeed - motorSpeed;
// Serial.print(sensors[0], sensors[1], sensors[2], sensors[3], sensors[4], sensors[5], sensors[6], sensors[7])
//code for sharp turn
if (sensors[0] < threshold && sensors[1] < threshold && sensors[2] < threshold && sensors[3] < threshold && sensors[4] < threshold && sensors[5] < threshold && sensors[6] < threshold && sensors[7] < threshold and test==1 ){
digitalWrite(13, HIGH);
Serial.println("Off-line");
if (LastLeftval < LastRightval) {
analogWrite(leftMotorPWM,reverse_speed);
analogWrite(rightMotorPWM,reverse_speed);
if (reverse_enable ==true){
digitalWrite(rightMotor1,LOW);
digitalWrite(rightMotor2, HIGH);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2,HIGH);
delay(turn_reversing_power_duration);
}
reverse_enable=false;
analogWrite(leftMotorPWM,rotation_speed);
analogWrite(rightMotorPWM,rotation_speed);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, HIGH);
delay(rotation_duration);
digitalWrite(rightMotor1,HIGH);
digitalWrite(rightMotor2, LOW);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2,LOW);
delay(30);
turn_counter=turn_counter+1;
// digitalWrite(rightMotor1, LOW);
// digitalWrite(leftMotor1, LOW);
} else if (LastRightval < LastLeftval) {
analogWrite(leftMotorPWM, reverse_speed);
analogWrite(rightMotorPWM,reverse_speed);
if (reverse_enable==true){
digitalWrite(rightMotor1,LOW);
digitalWrite(rightMotor2, HIGH);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2,HIGH);
delay(turn_reversing_power_duration);
}
reverse_enable =false;
analogWrite(leftMotorPWM,rotation_speed);
analogWrite(rightMotorPWM,rotation_speed);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, HIGH);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
delay(rotation_duration);
digitalWrite(rightMotor1,HIGH);
digitalWrite(rightMotor2, LOW);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2,LOW);
delay(30);
turn_counter=turn_counter+1;
// digitalWrite(rightMotor1, LOW);
// digitalWrite(leftMotor1, LOW);
}
} else {
if (rightMotorSpeed > rightMaxSpeed) rightMotorSpeed = rightMaxSpeed;
if (leftMotorSpeed > leftMaxSpeed) leftMotorSpeed = leftMaxSpeed;
if (rightMotorSpeed < 0) rightMotorSpeed = 0;
if (leftMotorSpeed < 0) leftMotorSpeed = 0;
//to slow down the car when exiting a turn
if (turn_counter>0){
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, 60);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, 60);
// delay(100);
}
else{
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
reverse_enable=true;
}
turn_off_counter = turn_off_counter+1;
}
LastLeftval = SumLeft;
LastRightval = SumRight;
if (turn_off_counter>=50){
turn_counter=0;
}
}
void wait() {
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
}
What changes can I make to make this work?
Any help would be appreciated.