       choice = 3;
       BTserial.flush();
     }
-
-    Serial.println(BluetoothData);
   }
 }
 

}

void loop() {
   switch (choice)
  {
    case 1:
      bluetoothcontrol(BluetoothData);

void headSetfxn()
{
  float nacc = acc.toFloat();
  hs.go_forward(nacc);
}

void aifxn()
{
  /*if (!go)
  {
    ai.go_forward();
    go = true;
    median = distanceSensor.getMedian();
    medianLeft = sensorLeft.getMedian();
    medianRight = sensorRight.getMedian();

  }*/
  ai.go_forward();
  median = distanceSensor.getMedian();
  medianLeft = sensorLeft.getMedian();
  medianRight = sensorRight.getMedian();

   if(medianLeft < TOO_CLOSE / 5)
    {
      ai.turn_right();
      delay(50);
      ai.brake();
      ai.turn_left();
      delay(5);
    }else if (medianRight < TOO_CLOSE / 5)
    {
      ai.turn_left();
      delay(50);
      ai.brake();
      ai.turn_right();
      delay(10);
    }


  if (median < CLOSE * 2) // medianFront
  {
    ai.brake();
    delay(500);
    brakeFlag = true;
    if(median < TOO_CLOSE/5)
    {
      ai.go_backward();
      delay(200);
      ai.brake();
    }
  }
  else
  {
    brakeFlag = false;
  }

  if (brakeFlag)
  {
    checkObstacles();
    brakeFlag = false;
    go = false;
  }

  aifxn();
}

void checkObstacles()
{
  median = distanceSensor.getMedian();
  medianLeft = sensorLeft.getMedian();
  medianRight = sensorRight.getMedian();

  if (medianLeft < TOO_CLOSE && medianRight < TOO_CLOSE)
  {
    ai.go_backward();
    checkObstacles();
  } else if (medianLeft < TOO_CLOSE && medianRight > TOO_CLOSE)
  {

    ai.turn_right();
    delay(270);
  } else if (medianLeft > TOO_CLOSE && medianRight < TOO_CLOSE)
  {

    ai.turn_left();
    delay(220);
  } else if (medianLeft > TOO_CLOSE && medianRight > TOO_CLOSE)
  {
    int r = random(2, 10);
    if (r % 2 == 0)
    {
      ai.turn_left();
      delay(220);
    } else if (r % 2 != 0)
    {
      ai.turn_right();
      delay(270);
    }
  }
}



// CODE TO WORK WITH ONE ULTRASONIC FRONT SENSOR ONLY
// The method will go forward until hits an obstacle
// When obstacle hit then turn left and see if there is obstacle there
// if there is none continue straight, if there is an obstacle
// turn right in order to come back to the same position
// then turn right again and see if there is obstacle on the right
// if there is none continue straight
// if there is an obstacle then turn left back to the initial position
// and start reversing and use recursive algorithm in order to check
// when the obstacles on the left or right side are gone
/*
  void aifxn()
  {
  if (!go)
  {
    ai.go_forward();
    go = true;
    median = distanceSensor.getMedian();
  }

  cm = distanceSensor.getMedian();

  if (cm < TOO_CLOSE)
  {
    ai.brake();
    delay(500);
    brakeFlag = true;
  }
  else
  {
    brakeFlag = false;
    go = false;
  }

  if (brakeFlag)
  {
    checkObstacles();
    brakeFlag = false;
  }
  }

  void checkObstacles()
  {
  checkObstacleLeft();

  if (obstacleLeft == true)
  {
    checkObstacleRight();
    delay(500);
    if (obstacleRight == true)
    {
      ai.go_backward();
      delay(1000); // Wait a second in order to brake
      ai.brake();
      checkObstacles();
    } else
    {
      ai.go_forward();
    }
  } else
  {
    //brakeFlag = false;
    //go = false;
  }
  }*/