obstacle_avoid_with_servo_look_left_right

#include <Servo.h>

// motor one
int enA = 12;    //enable PWM for right motor
int rightFwd = 7;    //right forward
int rightBack = 6;    //right backward
// motor two
int enB = 11;   //enable PWM for left motor
int leftFwd = 5;    //left forward
int leftBack = 4;    //left backward

//ultrasonic sensor
int trig = 2;
int echo = 3;

//servo for looking left and right
int servoPin = 8;

int runSpeed = 255;
int distance;
int leftDistance;
int rightDistance;
int servoCenter = 100;
int lookAngle = 40;

Servo servoMotor;

void setup()
{
  Serial.begin(9600);
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(rightFwd, OUTPUT);
  pinMode(rightBack, OUTPUT);
  pinMode(leftFwd, OUTPUT);
  pinMode(leftBack, OUTPUT);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(servoPin, OUTPUT);

  servoMotor.attach(servoPin);
  servoMotor.write(servoCenter);
}

void loop()
{
  if (getDistance() <= 15)
  {
    allStop();
    delay(60);

    if ((lookLeft() < 10) and (lookRight() < 10))
    {
      goBackward();
      delay(600);
      turnLeft();
      delay(300);
    }
    else if (lookLeft() > lookRight())
    {
      turnLeft();
      delay(200);
    }
    else
    {
      turnRight();
      delay(300);
    }
  }
  else
    goForward();

}

void goForward()
{
  Serial.println("Going forward");
  analogWrite(enA, runSpeed);
  analogWrite(enB, runSpeed);
  digitalWrite(leftBack,LOW);
  digitalWrite(leftFwd,HIGH);
  digitalWrite(rightBack,LOW);
  digitalWrite(rightFwd,HIGH);
}

void goBackward()
{
  Serial.println("Going backward");
  analogWrite(enA, runSpeed*3/4);
  analogWrite(enB, runSpeed*3/4);
  digitalWrite(leftBack,HIGH);
  digitalWrite(leftFwd,LOW);
  digitalWrite(rightBack,HIGH);
  digitalWrite(rightFwd,LOW);
}

void turnLeft()
{
  Serial.println("Turning left");
  analogWrite(enA, runSpeed*3/4);
  analogWrite(enB, runSpeed*3/4);
  digitalWrite(leftBack,HIGH);
  digitalWrite(leftFwd,LOW);
  digitalWrite(rightBack,LOW);
  digitalWrite(rightFwd,HIGH);
}

void turnRight()
{
  Serial.println("Turning right");
  analogWrite(enA, runSpeed*3/4);
  analogWrite(enB, runSpeed*3/4);
  digitalWrite(leftBack,LOW);
  digitalWrite(leftFwd,HIGH);
  digitalWrite(rightBack,HIGH);
  digitalWrite(rightFwd,LOW);
}

void allStop()
{
  Serial.println("Stopping");
  digitalWrite(leftBack,LOW);
  digitalWrite(leftFwd,LOW);
  digitalWrite(rightBack,LOW);
  digitalWrite(rightFwd,LOW);
}

int getDistance()
{
  //  Getting the distance from the sensor
  long duration, distance;
  digitalWrite(trig,HIGH);
  delayMicroseconds(1000);
  digitalWrite(trig, LOW);
  duration=pulseIn(echo, HIGH);
  distance =(duration/2)/29.1;
  Serial.print(distance);
  Serial.println(" cm");
  delay(10);
  return distance;
}

int lookRight()
{
  Serial.println("Looking right\n");
  servoMotor.write(50);
  delay(500);
  int distance = getDistance();
  delay(100);
  servoMotor.write(servoCenter - lookAngle);
  delay(100);
  servoMotor.write(servoCenter);
  return distance;
}

int lookLeft()
{
  Serial.println("Looking left\n");
  servoMotor.write(servoCenter + lookAngle);
  delay(500);
  int distance = getDistance();
  delay(100);
  servoMotor.write(servoCenter);
  return distance;
  delay(100);
}

void test()
{
  getDistance();
  goForward();
  delay(1000);
  allStop();
  delay(1000);
  goBackward();
  delay(1000);
  allStop();
  delay(1000);
  turnLeft();
  delay(1000);
  allStop();
  delay(1000);
  turnRight();
  delay(1000);
  allStop();
  delay(1000);
  lookLeft();
  lookRight();
}