dojo1.ino

#include <Servo.h>
#include <hcsr04.h>

Servo frontServo;
Servo backServo;
#define TRIG1 4
#define TRIG2 5
#define ECHO1 6
#define ECHO2 7
int ENA = 12;  //Left motor
int motorLeft1 = 1;  //Left motor
int motorLeft2 = 2;  //Left motor
int ENB = 10;  //Left motor
int motorRight1 = 3;  //Left motor
int motorRight2 = 11; //Right motor

bool foward;

float leftDist;
float frontDist;
float rightDist;

HCSR04 hcsr04a(TRIG1, ECHO2, 20, 4000);
HCSR04 hcsr04b(TRIG2, ECHO2, 20, 4000);

void turn_left() {
    digitalWrite(motorLeft1, HIGH);
    digitalWrite(motorLeft2, LOW);
    analogWrite(ENA, 125);
    digitalWrite(motorRight1, LOW);
    digitalWrite(motorRight2, HIGH);
    digitalWrite(ENB, 125);
    delay(200);  
}

void turn_right() {
    digitalWrite(motorLeft1, LOW);
    digitalWrite(motorLeft2, HIGH);
    analogWrite(ENA, 125);
    digitalWrite(motorRight1, HIGH);
    digitalWrite(motorRight2, LOW);
    digitalWrite(ENB, 125);
    delay(200);      
}

void reverse() {
    digitalWrite(motorLeft1, HIGH);
    digitalWrite(motorLeft2, HIGH);
    analogWrite(ENA, 125);
    digitalWrite(motorRight1, HIGH);
    digitalWrite(motorRight2, HIGH);
    digitalWrite(ENB, 125);
    delay(200);      
}

void forward() {
    digitalWrite(motorLeft1, LOW);
    digitalWrite(motorLeft2, LOW);
    analogWrite(ENA, 125);
    digitalWrite(motorRight1, LOW);
    digitalWrite(motorRight2, LOW);
    digitalWrite(ENB, 125);
    delay(200);      
}

void brake() {
    digitalWrite(motorLeft1, LOW);
    digitalWrite(motorLeft2, LOW);
    analogWrite(ENA, 0);
    digitalWrite(motorRight1, LOW);
    digitalWrite(motorRight2, LOW);
    digitalWrite(ENB, 0);
    delay(200);      
}

void setup() {
  frontServo.attach(8);
  backServo.attach(13);
  pinMode(motorRight1, OUTPUT);
  pinMode(motorRight2, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(motorLeft1, OUTPUT);
  pinMode(motorLeft2, OUTPUT);
  pinMode(ENB, OUTPUT);
  foward = true;
}

void loop() {
  if (foward) {
    brake();
    forward();
    frontServo.write(0);
    leftDist = hcsr04a.distanceInMillimeters();
    frontServo.write(90);
    frontDist = hcsr04a.distanceInMillimeters();
    frontServo.write(180);
    rightDist = hcsr04a.distanceInMillimeters();
    if (leftDist < 10 && frontDist < 10 && rightDist < 10) {
      foward = false;
    }
    if (leftDist > 10){
      brake();
      turn_left();
    }
    if (rightDist > 10) {
      brake();
      turn_right();
    }
    
  }else{
    brake();
    reverse();
    frontServo.write(0);
    leftDist = hcsr04a.distanceInMillimeters();
    frontServo.write(90);
    frontDist = hcsr04a.distanceInMillimeters();
    frontServo.write(180);
    rightDist = hcsr04a.distanceInMillimeters();
    if (leftDist < 10 && frontDist < 10 && rightDist < 10) {
      foward = true;
    }
    if (leftDist > 10){
      brake();
      turn_right();
    }
    if (rightDist > 10) {
      brake();
      turn_left();
    }
  }
  

}