Auto Obstacle Avoidance & Table Edge Detection Robot

#include <Servo.h>
#define BS 12      // back sensor 
#define FS 13      // front sensor 
#define L1 10       // left motor 
#define L2 11       // left motor 
#define R1 8       // right motor 
#define R2 9       // right motor 
#define E 2 // attach pin D2 Arduino to pin Echo of HC-SR04
#define T 3 //attach pin D3 Arduino to pin Trig of HC-SR04
#define S 5 // for speed of pin 5
#define SR 4// Servo
#define VIN A0
Servo srv;  // create servo object to control a servo
boolean isAuto = false;// Manual Default


void setup ()
{
  Serial.begin(9600);
  pinMode(FS, INPUT);
  pinMode(BS, INPUT);
  pinMode(L1, OUTPUT);
  pinMode(L2, OUTPUT);
  pinMode(R1, OUTPUT);
  pinMode(R2, OUTPUT);
  pinMode(S, OUTPUT);
  pinMode(T, OUTPUT);
  pinMode(E, INPUT);
  pinMode(VIN, INPUT);

  srv.attach(SR);
  melody();

}
const unsigned short melodyArr[] = {
  262, 196, 196, 220, 196, 0, 247, 262
};
const byte noteDurations[] = {
  250, 125, 125, 250, 250, 250, 250, 250
};
void melody() {
  for (int i = 0; i < 8; i++) {
    int noteDuration =  noteDurations[i];
    tone(6, melodyArr[i], noteDuration);
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
    noTone(6);
  }
}
unsigned int s = 255;
void loop()
{
  String str = "";
  char c;
  int di = ping();


  if (Serial.available()) {
    while (Serial.available()) {

      str = str + (char)Serial.read();

    }
    if (str.length() > 0) {
      c = str[0];
    }

    if (c == '1' && str.length() == 1) {
      move(true, false, true, false, s);
    } else if (c == '2' && str.length() == 1) {
      move(false, true, false, true, s);
    } else if (c == '3' && str.length() == 1) {
      move(true, false, false, true, s);// Left
    } else if (c == '4' && str.length() == 1) {
      move(false, true, true, false, s); // Right
    } else if (c == '5' && str.length() == 1) {
      move(false, false, false, false, s);
    } else if (str.startsWith("sp:")) {
      s = str.substring(3).toInt();
    } else if (str.startsWith("sr:")) {
      srv.write(str.substring(3).toInt());
    } else if (str.startsWith("tn:")) {
      tone(6, 480, str.substring(3).toInt());
    } else if (str.startsWith("mo:")) {
      isAuto = str.substring(3).toInt() == 0 ? false : true;
    } else if (str == "ml") {
      melody();
    } else if (str == "di") {
      Serial.println("di:" + String(di));
    } else if (str == "vin") {
      float vin = (analogRead(VIN) * 5.0) / 1023;
      Serial.println("vin:" + String(vin));
    }

  }
  char x = digitalRead(FS) ? '2' : '1';
  if ((di <= 20 || digitalRead(BS) || digitalRead(FS)) && c != x && !isAuto) {
    move(false, false, false, false, s);
  } else if (isAuto && (di <= 20 || digitalRead(FS))) {
    move(false, false, false, false, s);//Stop
    tone(6, 480, 1000);
    delay(1000);
    tone(6, 480, 1000);
    noTone(6);
    if ( ping() > 20 && !digitalRead(FS)) {
      delay(100);
      move(true, false, true, false, s);
    } else {
      delay(100);
      move(false, true, false, true, s);//Back
      delay(400);
      move(false, false, false, false, s);//Stop
      delay(300);
      int dRight = lookRight();
      delay(300);
      int dLeft = lookLeft();
      delay(300);
      if (dRight >= 20) {
        move(false, true, true, false, s); // Right
        delay(600);
        move(false, false, false, false, s);
      } else if (dLeft >= 20) {
        move(true, false, false, true, s);// Left
        delay(600);
        move(false, false, false, false, s);
      } else {
        move(false, false, false, false, s);//Stop
        melody();
      }

    }

  } else if (isAuto) {
    move(true, false, true, false, s);
  }
}
int lookRight() {
  srv.write(0);
  delay(300);
  int d = ping();
  srv.write(90);
  return d;
}
int lookLeft() {
  srv.write(180);
  delay(300);
  int d = ping();
  srv.write(90);
  return d;
}
int ping() {
  delay(100);

  // Clears the trigPin condition
  digitalWrite(T, LOW);
  delayMicroseconds(2);
  // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
  digitalWrite(T, HIGH);
  delayMicroseconds(10);
  digitalWrite(T, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  int du = pulseIn(E, HIGH);
  // Calculating the distance
  return du * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)
}
void move(bool l1, bool l2, bool r1, bool r2, int s) {

  analogWrite(S, s);
  digitalWrite(L1, l1);
  digitalWrite(L2, l2);
  digitalWrite(R1, r1);
  digitalWrite(R2, r2);


}