최종 프로젝트: 장애물을 피하는 로봇

// 장애물을 피하는 로봇: Arduino Uno + TB6612FNG + HC-SR04
const int TRIG = 7, ECHO = 8;
const int AIN1 = 2, AIN2 = 4, PWMA = 5;    // 왼쪽 모터
const int BIN1 = 10, BIN2 = 11, PWMB = 6;  // 오른쪽 모터
const int STBY = 12;                       // 드라이버 스탠바이

void setup() {
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  int pins[] = {AIN1, AIN2, PWMA, BIN1, BIN2, PWMB, STBY};
  for (int p : pins) pinMode(p, OUTPUT);
  digitalWrite(STBY, HIGH);  // 드라이버를 대기 모드에서 깨운다
  Serial.begin(9600);
}

long distanceCm() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);  // 10 µs 펄스: "측정해"
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  long us = pulseIn(ECHO, HIGH, 30000);  // 에코를 최대 30 ms 기다린다
  if (us == 0) return 400;               // 에코 없음: 길이 비어 있다
  return us / 58;                        // µs / 58 = cm
}

// speed는 -255(후진)부터 255(전진)까지
void motor(int in1, int in2, int pwm, int speed) {
  digitalWrite(in1, speed > 0);
  digitalWrite(in2, speed < 0);
  analogWrite(pwm, abs(speed));
}

void drive(int left, int right) {
  motor(AIN1, AIN2, PWMA, left);
  motor(BIN1, BIN2, PWMB, right);
}

void loop() {
  long d = distanceCm();
  Serial.println(d);
  if (d < 20) {
    drive(-150, -150);  // 후진
    delay(300);
    drive(150, -150);   // 제자리에서 회전
    delay(350);
  } else {
    drive(180, 180);    // 전진
  }
  delay(50);
}