#include <Servo.h>

// 超音波の宣言
int Trig = 8;
int Echo = 9;
double Duration;  // 超音波の間隔
double Distance;  // 距離
// 測定したい距離を下記に入力cm
int DetectionDistanceCm = 20;
// サーボの宣言
Servo MyservoLeft;
Servo MyservoRight;


void setup() {
  Serial.begin(9600);  
  // 超音波の初期設定
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);
  // サーボの初期設定
  ServoInitialSet(MyservoLeft, 2, 0);
  ServoInitialSet(MyservoRight, 3, 180);
}


void loop() {
  digitalWrite(Trig, LOW);
  delayMicroseconds(1);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(11);
  digitalWrite(Trig, LOW);
  Duration = pulseIn(Echo, HIGH);
  if(Duration > 0){
    Duration = Duration / 2; // 検知するのは往復併せた音波なので半分にする
    Distance = Duration * 340 * 100 / 1000000; // 音波は340m/sで進むのでそこから距離を割り出す
     if(Distance < DetectionDistanceCm){
      MyservoLeft.attach(2);
      MyservoRight.attach(3);
      MyservoLeft.write(180); // 左のサーボで引っ張った状態。引っ張りすぎる場合はここの数字を調整
      MyservoRight.write(0);  // 右のサーボで引っ張った状態
      delay(1000); 
      MyservoLeft.write(0);
      MyservoRight.write(180);
      delay(1000); 
      MyservoLeft.detach();
      MyservoRight.detach();
      } 
    }
  delay(500);
}


// servoの初期化設定
void ServoInitialSet(Servo Myservo, int AttachNumber, int SetAngle){
  Myservo.attach(AttachNumber);
  Myservo.write(SetAngle); 
  delay(500); 
  Myservo.detach();
  }