Overview
I recently purchased and assembled a line tracking robot car. (If it helps, it was the Elegoo UNO Project Smart Robot Car Kit V 3.0 with UNO R3.) I'm not interested in controlling the car with a remote or with a phone app, I am only interested in programming the car to work autonomously. The kit comes with a CD with various demonstration code on it. (And you can download the latest modifications from their website.)
When I try the line tracking sketch provided by the company, the results are fantastic. I am able to track a line without any problem. Here is a short video of the robot car running the default code.
And here is the default code I'm referring to.
//Line Tracking IO define
#define LT_R !digitalRead(10)
#define LT_M !digitalRead(4)
#define LT_L !digitalRead(2)
#define ENB 5
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11
#define ENA 6
#define carSpeed 200
void forward(){
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void back(){
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
void left(){
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void right(){
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
void stop(){
digitalWrite(ENA, LOW);
digitalWrite(ENB, LOW);
}
void setup(){
}
void loop() {
if(LT_M){
forward();
}
else if(LT_R) {
right();
while(LT_R);
}
else if(LT_L) {
left();
while(LT_L);
}
}
Getting to my main point and question
The default line tracking sketch does not make use of the Sonar Sensor on the front. As you can see from the code, the Sonar Sensor is nowhere to be found. I want to "simply" turn on the Sonar Sensor so that the car will stop if it detects an obstacle. (Eventually I want to do more, but my immediate short term milestone is to just have the car stop if there is an obstacle on the track.)
The changes I made to the default code are as minor as I can imagine them being while still having the functionality I want programmed in. However, when I add the Sonar into the equation, the car will occasionally do a 180 or 360 on the line rather than follow it, and worse, it will run off the track completely.
Here is another short video showing the car misbehaving once I've modified the code and added in Sonar awareness.
Here is the modified code.
#include <Servo.h>
Servo myservo;
//Line Tracking IO define
#define LT_R !digitalRead(10)
#define LT_M !digitalRead(4)
#define LT_L !digitalRead(2)
// Sonar define
#define Echo A4
#define Trig A5
// Servo define
#define SERVO_PIN 3
// Motor controller define
#define ENB 5
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11
#define ENA 6
#define carSpeed 200
int sonarDistance = 0;
const int distanceLimit = 25;
void forward(){
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void back(){
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
void left(){
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void right(){
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
void stop(){
digitalWrite(ENA, LOW);
digitalWrite(ENB, LOW);
}
//Ultrasonic distance measurement function
int getDistance() {
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
return (int)pulseIn(Echo, HIGH) / 58;
}
void setup(){
myservo.attach(SERVO_PIN);
pinMode(Echo, INPUT);
pinMode(Trig, OUTPUT);
stop();
myservo.write(90);
}
void loop() {
sonarDistance = getDistance();
if(sonarDistance <= distanceLimit && !(sonarDistance < 0)) {
// An object is blocking our path
stop();
} else {
if(LT_M){
forward();
}
else if(LT_R) {
right();
while(LT_R);
}
else if(LT_L) {
left();
while(LT_L);
}
}
}
I added awareness of the servo so that the sonar would center itself during setup(). Otherwise, the servo is not used.
In loop(), I've just added a couple of new lines to:
- Get sonar reading
- if object is detected, stop
- else, carry on with the default code
My guess is that it's an issue of timing. In the time it takes for getDistance() to take a reading, the car is speeding past the line.
To be clear, the sonar definitely works as can be seen in this short video clip.
If anyone has any ideas how to help, I would greatly appreciate it.
How can I have the line tracking work as flawlessly as it does in the first example while still having the Sonar Sensor enabled so that the car can stop if an object is placed on the track?
