// 서보모터로 초음파센서 움직여서 그 방향에있는 물건탐지 후 거리측정
#include <Servo.h>

const int echoPin  = 3;    //Echo 핀 할당
const int trigPin  = 4;    //Trig 핀 할당
const int servoPin = 5;    //Servo핀 할당

long duration;    //왕복시간
int distance;     //거리 cm

Servo myServo;    //서보모터 제어를 위한 객체 생성

void setup()
{
  pinMode(trigPin, OUTPUT);    //Trig 핀 output으로 세팅
  pinMode(echoPin, INPUT);     //Echo 핀 input으로 세팅
  Serial.begin(115200);        //시리얼 초기화
  //Serial.println("Distance Measure (Ultrasonic Sensor)");
  myServo.attach(servoPin);    //서보모터 연결핀
}

void loop()
{
  // 서보모터 15도~164도 회전
  for(int i=15; i<165; i++){  
    myServo.write(i);
    delay(30);
    distance = calculateDistance(); //초음파센서로 물체와의 거리 측정
    
    Serial.print(i);
    Serial.print(",");
    Serial.print(distance);
    Serial.print("\n");
  }
  // 서보모터 165도~16도 회전
  for(int i=165; i>15; i--){  
    myServo.write(i);
    delay(30);
    distance = calculateDistance(); //초음파센서로 물체와의 거리 측정
    
    Serial.print(i);
    Serial.print(",");
    Serial.print(distance);
    Serial.print("\n");
  }
}

int calculateDistance() 
{
  //Trig 핀으로 10us의 pulse 발생
  digitalWrite(trigPin, LOW);      //Trig 핀 Low
  delayMicroseconds(2);            //2us 유지
  digitalWrite(trigPin, HIGH);     //Trig 핀 High
  delayMicroseconds(10);           //10us 유지
  digitalWrite(trigPin, LOW);      //Trig 핀 Low

  //Echo 핀으로 들어오는 펄스의 시간 측정
  duration = pulseIn(echoPin, HIGH); //pulseIn함수가 호출되고 펄스가 입력될 때까지의 시간. us단위로 값을 리턴.

  //음파가 반사된 시간을 거리로 환산
  //음파의 속도는 340m/s 이므로 1cm를 이동하는데 약 29us
  //따라서, 물체와의 거리 = 왕복시간 * 음속(0.034) / 2
  //또는    물체와의 거리 = 왕복시간 / 음파이동시간(29) / 2
  distance = duration * 0.034 / 2;        //센치미터로 환산
  //distance = duration / 58.8;        //센치미터로 환산

  return distance;
}