// Obstacle Avoidance Robocar Arduino 2024
// Erica Stekman
// pengaturan parameter awal
#include <AFMotor.h> // library driver motor shield 4wd
#include <Servo.h> // library servo motor
#define Speed 90 // set kecepatan motor dc
#define Trig A1 // pin A1 arduino untuk pin Trigger sensor ultrasonic
#define Echo A0 // pin A0 arduino untuk pin Echo sensor ultrasonic
#define spoint 90 // set point posisi servo motor pada sudut 90 derajat
int distance; // sensor mulai membaca jarak
int Left; // memeriksa arah kiri
int Right; // memeriksa arah kanan
int L = 0; // menentukan jarak awal kiri = 0
int R = 0; // menentukan jarak awal kanan = 0
Servo servo; // set motor servo
AF_DCMotor M2(2); // terminal motor dc yang digunakan = M2
AF_DCMotor M3(3); // terminal motor dc yang digunakan = M3
// sintak yang dieksekusi hanya sekali
void setup(){ // sintak pengaturan utama
Serial.begin(9600); // setting untuk tampilan serial monitor
pinMode(Trig, OUTPUT); // pin trigger sensor sebagai output sinyal
pinMode(Echo, INPUT); // pin echo sensor sebagai output sinyal
servo.attach(10); // pin sinyal untuk servo
start(); // menyalakan servo
M2.setSpeed(Speed); // menyalakan motor dc M2
M3.setSpeed(Speed); // menyalakan motor dc M3
}
// sintak yang dieksekusi berulang
void loop(){ // sintak logika utama
distance = ultrasonic(); // jarak didapatkan dari sensor ultrasonic
Serial.println(distance); // menampilkan jarak pembacaan pada serial monitor
if (distance <=20){ // jika jarak kurang atau sama dengan 20cm
stop(); // robot berhenti
backward(); // robot mundur
delay(500); // selama 500ms
stop(); // robot berhenti
delay(200); // selama 200ms
L = leftsee(); // servo berputar, memeriksa arah kiri
servo.write(spoint); // servo kembali ke set point
delay(800); // tunggu 800ms
R = rightsee(); // servo berputar, memeriksa arah kanan
servo.write(spoint); // servo kembali ke set point
delay(800); // tunggu 800ms
if (L < R){ // jika kiri lebih kecil dari kanan
turnright(); // belok kanan
delay(500); // tunggu 500ms
stop(); // berhenti
delay(200); // tunggu 200ms
}
else if (L > R){ // jika kiri lebih besar dari kanan
turnleft(); // belok kiri
delay(500); // tunggu 500ms
stop(); // berhenti
delay(200); // tunggu 200ms
}
}
else{ // jika jarak awal lebih besar atau sama dengan 20cm
forward(); // robot bergerak maju
}
}
void forward(){ // sintak robot maju
M2.run(FORWARD); // motor dc 2 maju
M3.run(FORWARD); // motor dc 3 maju
}
void backward(){ // sintak robot mundur
M2.run(BACKWARD); // motor dc 2 mundur
M3.run(BACKWARD); // motor dc 3 mundur
}
void turnleft(){ // sintak robot belok kiri
M2.run(BACKWARD); // motor dc 2 mundur
M3.run(FORWARD); // motor dc 3 maju
}
void turnright(){ // sintak robot belok kanan
M2.run(FORWARD); // motor dc 2 maju
M3.run(BACKWARD); // motor dc 3 mundur
}
void stop(){ // sintak robot berhenti
M2.run(RELEASE); // motor dc 2 off
M3.run(RELEASE); // motor dc 3 off
}
int ultrasonic(){ // sintak pembacaan sensor ultrasonic
digitalWrite(Trig, LOW); // pin Trig non aktif (off)
delayMicroseconds(4); // tunggu 4ms
digitalWrite(Trig, HIGH); // pin Trig aktif (on)
delayMicroseconds(10); // tunggu 10ms
digitalWrite(Trig, LOW); // pin Trig non aktif (off)
long t = pulseIn(Echo, HIGH); // pin Echo menerima sinyal dari pin Trig
long cm = t / 29 / 2; // rumus pembacaan jarak berdasarkan waktu
penerimaan
return cm; // mendapatkan jarak cm
}
int leftsee(){ // sintak sensor memeriksa arah kiri
servo.write(10); // servo berputar ke sudut 10 derajat
delay(800); // tunggu 800ms
Left = ultrasonic(); // jarak arah kiri dibaca sensor
return Left; // mengirimkan hasil pembacaan jarak arah kiri
}
int rightsee(){ // sintak sensor memeriksa arah kanan
servo.write(170); // servo berputar ke sudut 170 derajat
delay(800); // tunggu 800ms
Right = ultrasonic(); // jarak arah kanan dibaca sensor
return Right; // mengirimkan hasil pembacaan jarak arah kanan
}
void start(){ // sintak memulai servo
delay(3000); // tunggu 3 detik
for (int a = 0; a < 4; a++);{ // operasi nyala (pengulangan) servo
servo.write(spoint); // servo ke posisi sudut set point
delay(50); // tunggu 50ms
servo.write(40); // servo ke posisi sudut 40 derajat
delay(50); // tunggu 50ms
servo.write(90); // servo ke posisi sudut 90 derajat
delay(50); // tunggu 50ms
servo.write(spoint); // servo ke posisi set point
}
}