Code for an obstacle avoidance robot using arduino and HC-SR04 sensor.
// And now with code tags - Moderator
//Code for arduino obstacle avoidance Robot. For tutorial, check out EngineerGeek on Youtube. Message with any queries. Thank you!
//Libraries that are included. Download from links found in description of video.
#include <AFMotor.h>
#include <Servo.h>
#include <NewPing.h>
// defining which pins our sensor is connected to. If you want to select different pins, change the A4 and A5 to whichever pins you have used.
#define TRIG_PIN A4
#define ECHO_PIN A5
//Sets our max distance.
#define MAX_DISTANCE_POSSIBLE 1000
//Setting the max speed of motors.
#define MAX_SPEED 150 //
#define MOTORS_CALIBRATION_OFFSET 3
//Our collision distance.
#define COLL_DIST 20
#define TURN_DIST COLL_DIST+10
//Ping for the HC-SR04.
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE_POSSIBLE);
//declaring our motors. If you have connected your motors on the motor shield to slot M1 and M2, just change the 4 and 3, to whichever slots you have used.
AF_DCMotor leftMotor(4, MOTOR12_8KHZ);
AF_DCMotor rightMotor(3, MOTOR12_8KHZ);
//Our servo.
Servo neckControllerServoMotor;
//creating different variables.
int pos = 0;
int maxDist = 0;
int maxAngle = 0;
int maxRight = 0;
int maxLeft = 0;
int maxFront = 0;
int course = 0;
int curDist = 0;
String motorSet = "";
int speedSet = 0;
//will run once upon initialization.
void setup() {
//Attaching our servo to digital pin 9, via the motor shield. Write(90) sets servo to face forward.
neckControllerServoMotor.attach(9);
neckControllerServoMotor.write(90);
delay(2000);
checkPath();
motorSet = "FORWARD";
neckControllerServoMotor.write(90);
moveForward();
}
//run continuously.
void loop() {
checkForward();
checkPath();
}
void checkPath() {
int curLeft = 0;
int curFront = 0;
int curRight = 0;
int curDist = 0;
//sweeps to one side.
neckControllerServoMotor.write(144);
delay(120);
//range the servo can move through.
for(pos = 144; pos >= 36; pos-=18)
{
//turns servo through steps.
neckControllerServoMotor.write(pos);
delay(90);
checkForward();
curDist = readPing();
if (curDist < COLL_DIST) {
checkCourse();
break;
}
if (curDist < TURN_DIST) {
changePath();
}
if (curDist > curDist) {maxAngle = pos;}
if (pos > 90 && curDist > curLeft) { curLeft = curDist;}
if (pos == 90 && curDist > curFront) {curFront = curDist;}
if (pos < 90 && curDist > curRight) {curRight = curDist;}
}
maxLeft = curLeft;
maxRight = curRight;
maxFront = curFront;
}
void setCourse() {
if (maxAngle < 90) {turnRight();}
if (maxAngle > 90) {turnLeft();}
maxLeft = 0;
maxRight = 0;
maxFront = 0;
}
void checkCourse() {
moveBackward();
delay(500);
moveStop();
setCourse();
}
void changePath() {
if (pos < 90) {lookLeft();}
if (pos > 90) {lookRight();}
}
//Our sensor reading in cm.
int readPing() {
delay(70);
unsigned int uS = sonar.ping();
int cm = uS/US_ROUNDTRIP_CM;
return cm;
}
void checkForward() { if (motorSet=="FORWARD") {leftMotor.run(FORWARD); rightMotor.run(FORWARD); } }
void checkBackward() { if (motorSet=="BACKWARD") {leftMotor.run(BACKWARD); rightMotor.run(BACKWARD); } }
void moveStop() {leftMotor.run(RELEASE); rightMotor.run(RELEASE);}
//our move forward function.
void moveForward() {
motorSet = "FORWARD";
leftMotor.run(FORWARD);
rightMotor.run(FORWARD);
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
{
leftMotor.setSpeed(speedSet+MOTORS_CALIBRATION_OFFSET);
rightMotor.setSpeed(speedSet);
delay(5);
}
}
//our move backward function.
void moveBackward() {
motorSet = "BACKWARD";
leftMotor.run(BACKWARD);
rightMotor.run(BACKWARD);
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
{
leftMotor.setSpeed(speedSet+MOTORS_CALIBRATION_OFFSET);
rightMotor.setSpeed(speedSet);
delay(5);
}
}
//our turn right function.
void turnRight() {
motorSet = "RIGHT";
leftMotor.run(FORWARD);
rightMotor.run(BACKWARD);
delay(400);
motorSet = "FORWARD";
leftMotor.run(FORWARD);
rightMotor.run(FORWARD);
}
//our turn left function.
void turnLeft() {
motorSet = "LEFT";
leftMotor.run(BACKWARD);
rightMotor.run(FORWARD);
delay(400);
motorSet = "FORWARD";
leftMotor.run(FORWARD);
rightMotor.run(FORWARD);
}
void lookRight() {rightMotor.run(BACKWARD); delay(400); rightMotor.run(FORWARD);}
void lookLeft() {leftMotor.run(BACKWARD); delay(400); leftMotor.run(FORWARD);}
