/*if someone approaches inside 1M, open two doors, 
else if someone goed outside 2M, close two doors
*/
#include <Servo.h>

Servo myServo, myServo2;
const int Servo_pin=9, Servo2_pin=8;
const int TRIG=7,ECHO=6;
const int LED_G=2, LED_R=3;

void setup() {
  myServo.attach(Servo_pin);
  myServo2.attach(Servo2_pin);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(LED_G,OUTPUT);
  pinMode(LED_R, OUTPUT);
  Serial.begin(9600);
}

double dist(){
  digitalWrite(TRIG,LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  //
  long dist=pulseIn(ECHO,HIGH);
  double ntemp=(double)(dist)*0.017;
  //
  return ntemp;
}

void loop() {
  /*
  int nVer=analogRead(A0);
  int nHor=analogRead(A1);
  int nX=map(nHor,0,1023,0,180);
  int nY=map(nVer,0,1023,0,180);
  //
  myServo.write(nX);
  myServo2.write(nY);
  //
  Serial.print("nX=");
  Serial.print(nX);
  Serial.print("\tnY=");
  Serial.println(nY);
  //
  delay(1000);
  */
  double dbitemp=dist();
  int ndist=(int)(dbitemp);
  Serial.print("Dist>>> ");
  Serial.print(ndist);
  Serial.println(" cm");
  static bool bflag=false;
  if(ndist<=100){
    myServo.write(180);
    myServo2.write(180);
    bflag=true;
  }
  else if(ndist>=200){
    myServo.write(0);
    myServo2.write(0);
    bflag=false;
  }
  if(bflag==true){
    digitalWrite(LED_G, HIGH);
    digitalWrite(LED_R, LOW);
  }
  else{
    digitalWrite(LED_G, LOW);
    digitalWrite(LED_R, HIGH);
  }
  //
  if(ndist<=100){
    myServo.write(180);
    myServo2.write(180);
  }
  else if(ndist>=200){
    myServo.write(0);
    myServo2.write(0);
  }
  //
  delay(100);
}