#include <Servo.h>


// 定義超音波感測器接腳
const int echoPin = 4;
const int triggerPin = 5;

// 定義伺服馬達初始位置
Servo servo_0;
const int initialServoPosition = 0;

// 定義距離值
const int distanceThreshold = 20;//距離20公分

void setup()
{
  // 初始化伺服馬達
  servo_0.attach(3, 500, 2500);
  servo_0.write(initialServoPosition);

  // 初始化超音波感測器接腳
  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // 初始化控制LED的接腳
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
}

long medirDistancia(int triggerPin, int echoPin)
{
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  
  digitalWrite(triggerPin, LOW);
  
  int duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.034 / 2;
  
  return distance;
}

void loop()
{
  // 測量距離
  long distance = medirDistancia(triggerPin, echoPin);
  
  // 根據距離控制伺服馬達和LED
  if (distance > distanceThreshold) //0度
  {
    servo_0.write(initialServoPosition);
    digitalWrite(7, HIGH);
    digitalWrite(6, LOW);
  }
  else
  {
    servo_0.write(90);
    digitalWrite(6, HIGH);
    digitalWrite(7, LOW);
    delay(3000);
  }
  
  delay(100);
}