Skip to content

Instantly share code, notes, and snippets.

@andreaschandra
Created June 27, 2025 03:35
Show Gist options
  • Save andreaschandra/60bc43c34c020777d198e465a321b63b to your computer and use it in GitHub Desktop.
Save andreaschandra/60bc43c34c020777d198e465a321b63b to your computer and use it in GitHub Desktop.
#include <Servo.h> //Servo motor library. This is standard library
#include <NewPing.h> //Ultrasonic sensor function library. You must install this library
//our L298N control pins
const int enAPin = 3;
const int enBPin = 8;
const int LeftMotorForward = 4;
const int LeftMotorBackward = 5;
const int RightMotorForward = 7;
const int RightMotorBackward = 6;
//sensor pins
#define trig_pin 13 //analog input 1
#define echo_pin 12 //analog input 2
#define maximum_distance 200
boolean goesForward = false;
int distance = 100;
NewPing sonar(trig_pin, echo_pin, maximum_distance); //sensor function
Servo servo_motor; //our servo name
void setup(){
Serial.begin(9600);
pinMode(RightMotorForward, OUTPUT);
pinMode(LeftMotorForward, OUTPUT);
pinMode(LeftMotorBackward, OUTPUT);
pinMode(RightMotorBackward, OUTPUT);
pinMode(enAPin, OUTPUT);
pinMode(enBPin, OUTPUT);
digitalWrite(enAPin, HIGH);
digitalWrite(enBPin, HIGH);
servo_motor.attach(11); //our servo pin
servo_motor.write(90);
delay(2000);
servo_motor.write(0);
delay(2000);
servo_motor.write(180);
delay(2000);
servo_motor.write(90);
delay(2000);
}
void loop(){
distance = readPing();
int distanceRight = 0;
int distanceLeft = 0;
Serial.print("distance: ");
Serial.println(distance);
if (distance <= 20){
moveStop();
delay(300);
moveBackward();
delay(300);
moveStop();
delay(300);
distanceRight = lookRight();
distanceLeft = lookLeft();
delay(300);
if (distance >= distanceLeft){
turnRight();
moveStop();
} else {
turnLeft();
moveStop();
}
} else {
moveForward();
}
}
int lookRight(){
servo_motor.write(10);
delay(500);
int distance = readPing();
delay(100);
servo_motor.write(90);
return distance;
}
int lookLeft(){
servo_motor.write(170);
delay(500);
int distance = readPing();
delay(100);
servo_motor.write(90);
return distance;
delay(100);
}
int readPing(){
delay(70);
int cm = sonar.ping_cm();
if (cm==0){
cm=250;
}
return cm;
}
void moveStop(){
digitalWrite(RightMotorForward, LOW);
digitalWrite(LeftMotorForward, LOW);
digitalWrite(RightMotorBackward, LOW);
digitalWrite(LeftMotorBackward, LOW);
}
void moveForward(){
Serial.println("Trigger moveForward");
digitalWrite(LeftMotorForward, HIGH);
digitalWrite(RightMotorForward, HIGH);
digitalWrite(LeftMotorBackward, LOW);
digitalWrite(RightMotorBackward, LOW);
}
void moveBackward(){
goesForward=false;
Serial.println("Trigger moveBackward");
digitalWrite(LeftMotorBackward, HIGH);
digitalWrite(RightMotorBackward, HIGH);
digitalWrite(LeftMotorForward, LOW);
digitalWrite(RightMotorForward, LOW);
}
void turnRight(){
digitalWrite(LeftMotorForward, HIGH);
digitalWrite(RightMotorBackward, HIGH);
digitalWrite(LeftMotorBackward, LOW);
digitalWrite(RightMotorForward, LOW);
delay(500);
digitalWrite(LeftMotorForward, HIGH);
digitalWrite(RightMotorForward, HIGH);
digitalWrite(LeftMotorBackward, LOW);
digitalWrite(RightMotorBackward, LOW);
}
void turnLeft(){
digitalWrite(LeftMotorBackward, HIGH);
digitalWrite(RightMotorForward, HIGH);
digitalWrite(LeftMotorForward, LOW);
digitalWrite(RightMotorBackward, LOW);
delay(500);
digitalWrite(LeftMotorForward, HIGH);
digitalWrite(RightMotorForward, HIGH);
digitalWrite(LeftMotorBackward, LOW);
digitalWrite(RightMotorBackward, LOW);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment