#include<Servo.

h>
#define trigPin 7 //pin trig dari sensor jarak
#define echoPin 4 //pin echo dari sensor jarak
#define m1 10
#define m2 11
#define m3 8
#define m4 9
const int RForward = 120; //kecepatan servo, kecepatan maksimum adalah 180
const int RBackward = 60;
const int LForward = 60;
const int LBackward = 120;
const int RNeutral = 90; //posisi berpusat
const int LNeutral = 90;
const int RightLightSensor = 0; //menyatakan pin analog untuk photoresistors
const int LeftLightSensor = 2;
const int collisionThresh = 15; //batas untuk hambatan (dalam cm)
int SensorLeft;
int SensorRight;
int SensorDifference;
int leftDistance, rightDistance; //jarak di kedua sisi
Servo panMotor; //servo mikro dengan sensor jarak di atasnya
long duration; //waktu yang dibutuhkan untuk menerima PING))) sinyal
void setup() {
Serial.begin(9600);
panMotor.attach(6);
panMotor.write(90); //pusat pan servo
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(m1, OUTPUT);
pinMode(m2, OUTPUT);
pinMode(m3, OUTPUT);
pinMode(m4, OUTPUT);
pinMode(LeftLightSensor, INPUT);
pinMode(RightLightSensor, INPUT);
}
void loop(){
int distance = ping(); //memanggil fungsi ping untuk mendapatkan jarak di depan
robot
if (distance > collisionThresh)
{
if (SensorDifference > 75) { //kiri
digitalWrite(m1,HIGH);//maju
digitalWrite(m2,LOW);
digitalWrite(m3,HIGH);
digitalWrite(m4,LOW);
delay(250);
}
if (SensorDifference > 75) { //Kanan
digitalWrite(m1,LOW);
digitalWrite(m2,HIGH);
digitalWrite(m3,LOW);
digitalWrite(m4,HIGH);
delay(250);
}
else if (SensorDifference < 75) { //depan
digitalWrite(m1,HIGH);
digitalWrite(m2,LOW);
digitalWrite(m3,LOW);
digitalWrite(m4,HIGH);
delay(500);
}
}
else {
digitalWrite(m1,LOW);
digitalWrite(m2,LOW);
digitalWrite(m3,LOW);
digitalWrite(m4,LOW);
panMotor.write(0);
delay(500);
rightDistance = ping(); //memindai ke kanan
delay(500);
panMotor.write(180);
delay(700);
leftDistance = ping(); //memindai ke kiri
delay(500);
panMotor.write(90); //kembali ke pusat
delay(100);
compareDistance();
}
}
void compareDistance()
{
if (leftDistance > rightDistance) //jika terhalang
{
digitalWrite(m1,LOW);
digitalWrite(m2,HIGH);
digitalWrite(m3,LOW);
digitalWrite(m4,HIGH);
delay(500);
}
else if (rightDistance > leftDistance) //jika benar terhalang
{
digitalWrite(m1,HIGH);
digitalWrite(m2,LOW);
digitalWrite(m3, HIGH);
digitalWrite(m4,LOW);
delay(500);
}
else //jika mereka sama-sama terhalang
{
digitalWrite(m1,HIGH);
digitalWrite(m2,HIGH);
digitalWrite(m3,HIGH);
digitalWrite(m4, HIGH);
delay(500);
}
}
long ping() { // berakhir keluar PING))) pulsa sinyal
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(5);
digitalWrite(trigPin, LOW);
//Dapatkan durasi yang dibutuhkan untuk menerima echo
duration = pulseIn(echoPin, HIGH);
//Mengkonversi durasi kejauhan
return duration / 75 / 2;
}