#include <Servo.h>

Servo servoLeft;
Servo servoRight;

const int leftWhiskerPin = 5;   // 안쪽 더듬이
const int rightWhiskerPin = 7;  // 바깥쪽 더듬이

void setup() {
  servoLeft.attach(13);
  servoRight.attach(12);

  pinMode(leftWhiskerPin, INPUT_PULLUP);
  pinMode(rightWhiskerPin, INPUT_PULLUP);
}

void loop() {
  // ----------------------------------------
  // 1️⃣ 기본 원 주행: 시계 방향 원
  //    왼쪽 최대 1700, 오른쪽 1300 → 전진 + 원 주행
  // ----------------------------------------
  servoLeft.writeMicroseconds(1700);
  servoRight.writeMicroseconds(1300);

  // ----------------------------------------
  // 2️⃣ 안쪽 더듬이 터치 → 작은 원
  //    오른쪽 바퀴가 정지(1500)될 때까지 점진 변화
  // ----------------------------------------
  if (digitalRead(leftWhiskerPin) == LOW) {
    for (int i = 0; i <= 50; i++) {        // 단계별 PWM 변화
      servoLeft.writeMicroseconds(1700);   // 왼쪽 최대
      servoRight.writeMicroseconds(1300 + i); // 오른쪽 1300 → 1500 정지
      delay(50);                            // 50ms 반복

      // 조건: 오른쪽 PWM이 1500 도달 시 종료
      if (servoRight.readMicroseconds() >= 1500) break;
    }
  }

  // ----------------------------------------
  // 3️⃣ 바깥쪽 더듬이 터치 → 큰 원
  //    직선이 될 때까지 PWM 점진 변화
  // ----------------------------------------
  if (digitalRead(rightWhiskerPin) == LOW) {
    for (int i = 0; i <= 70; i++) {        // 단계별 PWM 변화
      servoLeft.writeMicroseconds(1700);   // 왼쪽 최대
      servoRight.writeMicroseconds(1300 + i); // 오른쪽 1300 → 1700 (직선)
      delay(50);

      // 조건: 양쪽 PWM이 같아지면 직선 완료
      if (servoRight.readMicroseconds() >= 1700) break;
    }
  }
}