I’m using an Arduino Uno, an HC-SR04 ultrasonic sensor mounted on a servo, and an L298n dual h-bridge motor controller. The L298n is powered with a 7.2v battery pack and the Uno with a 9V (or usb for testing).
The goal is like most other obstacle avoiding robots. It should check the distance in front and move forward as long as the measured distance is greater than the distanceLimit. If the distance is less than distanceLimit, the robot should stop, then take a measurement to the left and right, then compare. To keep it simple, if one side measures higher than the other then the robot turns that direction and starts the loop over again.
Unfortunately, the results are so varied that I can’t pin down what the issues are. Most times the bot will run right into an object or wall, sometimes stopping other times not. Other times the robot won’t even start moving despite receiving values greater than the distanceLimit (as confirmed by serial monitor). There are also times where it will seem to get stuck in a “look around” mode and/or keep turning, stopping, looking, then turning again. This is especially interesting in a hallway where it seems unable to just move down a hallway. The other concern is that despite which of my sensors I’m using I still get quite a few 0’s.
So my first question is whether or not I’ve missed a condition in my loop that causes it to get stuck in the “look around” and/or turn mode instead of moving forward when it should be clear.
#include <NewPing.h>
#include <Servo.h>
#define TRIGGER_PIN 12
#define ECHO_PIN 13
#define MAX_DISTANCE 50
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // distance sensor
int enableA = 11; // H bridge pins
int pinIN1 = 6;
int pinIN2 = 5;
int enableB = 10;
int pinIN3 = 4;
int pinIN4 = 3;
Servo head; // servo naming
const int distanceLimit = 20; // collision distance limit
unsigned int frontcm; // distance measurements
unsigned int leftcm;
unsigned int rightcm;
void setup() {
Serial.begin(9600);
head.attach(2);
head.write(90);
pinMode(enableA, OUTPUT);
pinMode(pinIN1, OUTPUT);
pinMode(pinIN2, OUTPUT);
pinMode(enableB, OUTPUT);
pinMode(pinIN3, OUTPUT);
pinMode(pinIN4, OUTPUT);
enableMotors();
}
void loop() {
delay(50); // this delay is supposedly for pings?
frontcm = sonar.ping_cm(); // take a front measurement
if (frontcm > distanceLimit || frontcm == 0) {
forward;
}
else {
stopMove();
checkPath();
turnDirection();
}
}
void checkPath() {
head.write(170); // look left
delay(300); // wait for servo to turn
leftcm = sonar.ping_cm(); // take left measurement
head.write(10); // now turn right
delay(300);
rightcm = sonar.ping_cm();
head.write(90); //return to front
delay(200);
}
void turnDirection() {
if (leftcm > rightcm || leftcm == 0) {
turnLeft();
delay(200);
}
else if (rightcm > leftcm || rightcm == 0) {
turnRight();
delay(200);
}
else if (leftcm < distanceLimit && rightcm < distanceLimit) {
reverse();
delay(400);
}
}
// high level motor functions
void enableMotors() {
motorAon();
motorBon();
}
void forward() {
motorAforward();
motorBforward();
}
void reverse() {
motorAreverse();
motorBreverse();
}
void turnLeft() {
motorAreverse();
motorBforward();
}
void turnRight() {
motorAforward();
motorBreverse();
}
void stopMove() {
motorAstop();
motorBstop();
}
// low level motor functions
void motorAon() {
digitalWrite(enableA, HIGH);
}
void motorBon() {
digitalWrite(enableB, HIGH);
}
void motorAforward() {
digitalWrite(pinIN1, HIGH);
digitalWrite(pinIN2, LOW);
}
void motorBforward() {
digitalWrite(pinIN3, HIGH);
digitalWrite(pinIN4, LOW);
}
void motorAreverse() {
digitalWrite(pinIN1, LOW);
digitalWrite(pinIN2, HIGH);
}
void motorBreverse() {
digitalWrite(pinIN3, LOW);
digitalWrite(pinIN4, HIGH);
}
void motorAstop() {
digitalWrite(pinIN1, HIGH);
digitalWrite(pinIN2, HIGH);
}
void motorBstop() {
digitalWrite(pinIN3, HIGH);
digitalWrite(pinIN4, HIGH);
}
