#include <ESP8266WiFi.h>
#include <Servo.h>

#define TRIGGER_PIN  12
#define ECHO_PIN     13
#define TRIG2 5
#define ECHO2 4

#define SERVO_PIN  15

float duration_us, distance_cm, depth_cm, duration_in;
Servo myservo;

const int DISTANCE_THRESHOLD = 10;
int servoNow = 180;

const char* ssid = "iPhone"; //WiFi Name
const char* password = "alfialfi"; // WiFi Password

void setup()
{
  Serial.begin(74880);

   pinMode(TRIG2, OUTPUT);
  pinMode(ECHO2, INPUT);
  
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  myservo.attach(SERVO_PIN);
  myservo.write(180);
  
  Serial.printf("Connecting to %s ", ssid);
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED)
    {
    delay(500);
    Serial.print(".");
    }
    Serial.println(" CONNECTED");

}

void loop()
{

  read_distance();

  Serial.print (duration_us);
  Serial.print ("    ");
  Serial.println (distance_cm);
  Serial.println (duration_in);
   Serial.println (depth_cm);
  
  if (distance_cm <= DISTANCE_THRESHOLD)
  {
    moveTo(90);
    delay(5000);
    moveTo(180);
  }

  delay(500);
}



void moveTo(int angle) {

  int error = angle - servoNow;
  int inc = (error > 0) ? 1 : -1;

  for (int i = 0 ; i < abs(error); i++) {
    servoNow += inc;
    myservo.write(servoNow);
    delay(10);
  }
}


void read_distance() {
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);

  duration_us = pulseIn(ECHO_PIN, HIGH);

  // calculate the distance

  distance_cm = 0.017 * duration_us;

   digitalWrite(TRIG2, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG2, LOW);

  duration_in = pulseIn(ECHO2, HIGH);
  
  depth_cm = 0.017 * duration_in;
}