// Pin Driver Motor
const int IN1 = 10;
const int IN2 = 9;
const int IN3 = 6;
const int IN4 = 5;
// Pin LED
const int led_red = 2;
const int led_yellow = 3;
const int led_green = 4;
// Pin Switch
const int switch_button = 12;
// Pin Passive Buzzer
const int buzzer = 11;
// Pin Ultrasonic Sensor
const int trig = 7;
const int echo = 8;
int switch_value = 0;
int speed_left = 120; //0-255
int speed_right = 120; //0-255
float duration, distance;
bool turn = false;
void read_distance();
void forward();
void backward();
void left();
void right();
void turnleft();
void turnright();
void setup() {
Serial.begin(115200);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(led_red, OUTPUT);
pinMode(led_yellow, OUTPUT);
pinMode(led_green, OUTPUT);
pinMode(switch_button, INPUT);
pinMode(buzzer, OUTPUT);
pinMode(trig, OUTPUT);
pinMode(echo, INPUT);
noTone(buzzer);
analogWrite(IN1, 0); analogWrite(IN3, 0);
analogWrite(IN2, 0); analogWrite(IN4, 0);
tone(buzzer, 300); delay(100); noTone(buzzer); delay(100);
while (digitalRead(switch_button) == 1) {
;
}
tone(buzzer, 400); delay(100); noTone(buzzer); delay(100);
tone(buzzer, 400); delay(100); noTone(buzzer); delay(500);
}
void loop() {
read_distance();
Serial.print("Distance: ");
Serial.println(distance);
delay(100);
if (distance > 20) {
forward(); Serial.println("forward");
digitalWrite(led_red, LOW);
digitalWrite(led_yellow, LOW);
digitalWrite(led_green, HIGH);
}
else if ((distance < 20)&&(turn == false)) {
Serial.println("trunleft");
tone(buzzer, 300); delay(50); noTone(buzzer); delay(50);
tone(buzzer, 600); delay(50); noTone(buzzer); delay(50);
tone(buzzer, 900); delay(50); noTone(buzzer); delay(50);
digitalWrite(led_red, HIGH);
digitalWrite(led_yellow, LOW);
digitalWrite(led_green, LOW);
stopAll(); delay(100);
digitalWrite(led_red, LOW);
digitalWrite(led_yellow, HIGH);
digitalWrite(led_green, LOW);
backward(); delay(300);
turnleft(); delay(400);
stopAll(); delay(200);
if(turn == false){
turn = true;
}
}
else if ((distance < 20)&&(turn == true)) {
Serial.println("trunright");
tone(buzzer, 900); delay(50); noTone(buzzer); delay(50);
tone(buzzer, 600); delay(50); noTone(buzzer); delay(50);
tone(buzzer, 300); delay(50); noTone(buzzer); delay(50);
digitalWrite(led_red, HIGH);
digitalWrite(led_yellow, LOW);
digitalWrite(led_green, LOW);
stopAll(); delay(100);
digitalWrite(led_red, LOW);
digitalWrite(led_yellow, HIGH);
digitalWrite(led_green, LOW);
backward(); delay(300);
turnright(); delay(800);
stopAll(); delay(200);
if(turn == true){
turn = false;
}
}
}
void read_distance() {
digitalWrite(trig, LOW);
delayMicroseconds(2);
digitalWrite(trig, HIGH);
delayMicroseconds(10);
digitalWrite(trig, LOW);
duration = pulseIn(echo, HIGH);
distance = (duration * .0343) / 2;
}
void forward() {
analogWrite(IN1, speed_left); analogWrite(IN3, speed_right);
analogWrite(IN2, 0); analogWrite(IN4, 0);
}
void backward() {
analogWrite(IN1, 0); analogWrite(IN3, 0);
analogWrite(IN2, speed_left); analogWrite(IN4, speed_right);
}
void left() {
analogWrite(IN1, 0); analogWrite(IN3, speed_right);
analogWrite(IN2, 0); analogWrite(IN4, 0);
}
void right() {
analogWrite(IN1, speed_left); analogWrite(IN3, 0);
analogWrite(IN2, 0); analogWrite(IN4, 0);
}
void turnleft() {
analogWrite(IN1, 0); analogWrite(IN3, speed_right);
analogWrite(IN2, speed_left); analogWrite(IN4, 0);
}
void turnright() {
analogWrite(IN1, speed_left); analogWrite(IN3, 0);
analogWrite(IN2, 0); analogWrite(IN4, speed_right);
}
void stopAll() {
analogWrite(IN1, speed_left); analogWrite(IN3, speed_right);
analogWrite(IN2, speed_left); analogWrite(IN4, speed_right);
}
