Created
June 27, 2025 03:35
-
-
Save andreaschandra/60bc43c34c020777d198e465a321b63b to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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