Arduino Obstacle Avoiding Robot - w/ Custom Chassis

#include<Servo.h>
const int trigPin = 4;    // Trigger pin
const int echoPin = 2;   // Echo pin
const int servo = 3;      //servo motor
const int led = 10; //LED
//motor control variables
const int ENA = 5;
const int in1 = 7;
const int in2 = 8;
const int in3 = 13;
const int in4 = 12;
const int ENB = 6;

Servo scanServo;


void setup() {
  Serial.begin(9600); //allows u to track serial values
  pinMode(trigPin, OUTPUT); //because this is a digital pin, u have to set it as input/output in the setup
  pinMode(echoPin, INPUT); //this is done for both pins
  pinMode (led,OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  analogWrite(ENA, 200); //right
  analogWrite(ENB, 210); //left 

  scanServo.attach(servo);
}



long sensor () {
  long duration, cm; //setting 2 variables, one for how long the light takes, and the other for how much the ligh travels

  // Clear the trigger
  digitalWrite(trigPin, LOW); //trigger pin (the one sending out pulse) is turned off
  delayMicroseconds(2);

  // Send the 10us pulse to trigger
  digitalWrite(trigPin, HIGH); //trigger pin turns on, meaning a pulse is sent out
  delayMicroseconds(10); //the pulse is sent out for 10ms
  digitalWrite(trigPin, LOW); //trigger pin is turned off again

  // Read echo time
  duration = pulseIn(echoPin, HIGH);//measures how long it takes for the pulse to come back to the echo pin
  //echoPin goes HIGH when trigger pin sends out pulse, echoPin turns LOW right as it receives back the pulse, the pulseIn command measures how long that duration is in microseconds

  // Convert to distance in cm
  cm = duration / 29 / 2; //converts the time to distance 

  // Print result
  Serial.print(cm); //prints out cm in serial 

  return cm;

}

void straight () { //fucntion to go straight
  digitalWrite(in1, HIGH);
  digitalWrite(in2,LOW);

  digitalWrite(in3, HIGH);
  digitalWrite(in4,LOW);

}

void backwards () { //function to go backwards (when object is detected)
  digitalWrite(in1, LOW);
  digitalWrite(in2,HIGH);

  digitalWrite(in3, LOW);
  digitalWrite(in4,HIGH);

}

void left () { //function to turn left
  digitalWrite(in1, HIGH);
  digitalWrite(in2,LOW);

  digitalWrite(in3, LOW);
  digitalWrite(in4,HIGH);

}

void right () { //function to turn right
  digitalWrite(in1, LOW);
  digitalWrite(in2,HIGH);

  digitalWrite(in3, HIGH);
  digitalWrite(in4,LOW);

}

void stop (){ //function to stop
  digitalWrite(in1, LOW);
  digitalWrite(in2,LOW);

  digitalWrite(in3, LOW);
  digitalWrite(in4,LOW);
}


void loop() {
  scanServo.write(90); //faces forward
  delay(500);
  long frontDis = sensor(); //determines front distance
  Serial.println(" front");
  

  scanServo.write(30); //turns right
  delay(500);
  long rightDis = sensor(); //determines right distance
  Serial.println(" right");
    
  scanServo.write(150); //turns left
  delay(500);
  long leftDis = sensor(); //determines left distance
  Serial.println(" left");

  scanServo.write(90); //turns straight again
  delay(500);
  
  if(frontDis>30){

    if(frontDis>100){ //goes straight for 3 seconds if there is no object within 1m
      straight();
      delay(3000);
      stop();

    }

    else if(frontDis>60){ //goes straight for 1.4 seconds if there is not object within 0.6m
    straight();
    delay (1400);
    stop();
    }

    else{ //goes staight for 0.7s if there is no object within 0.3m
      straight();
      delay (700);
      stop();
    }

  }

  else { //if object is detected within 0.3m, then turn left/right
    
    digitalWrite(led,HIGH); //lED turned on the indicate to user that an object has been detected, and a turn will be taking place

    if (rightDis>leftDis){ //turns right
      backwards();
      delay(500);
      delay(100);
      right();
      delay(400);

    }
    else if (leftDis>rightDis) { //turns left
      backwards();
      delay(500);
      delay(100);
      left();
      delay(400);
    }

    stop(); 

    digitalWrite(led,LOW); //LED is switched off after the robot turns

  }
}