car

//오른쪽 바퀴
int A_1A = 6;
int A_1B = 11;

//오른쪽 바퀴
int B_1A = 3;
int B_1B = 5;

/*
   초음파센서
   초음파센서 TRIG - D8
   초음파센서 ECHO - D9
*/
int trigPin = 8;
int echoPin = 9;

int speed = 100;    //자동차 속도
int speedPin = 100;
long duration = 0;
long leftDistance = 0;
long rightDistance = 0;

/**
   왼쪽,오른쪽 모터를 정회전하여 전진합니다.
*/
void forward() {
  //모터A 정회전
  analogWrite(A_1A, speed);
  analogWrite(A_1B, 0);
  //모터B 정회전
  analogWrite(B_1A, speed);
  analogWrite(B_1B, 0);
}

/**
   왼쪽,오른쪽 모터를 역회전하여 후진합니다.
*/
void back() {
  //모터A 역회전
  analogWrite(A_1A, 0);
  analogWrite(A_1B, speed);
  //모터B 역회전
  analogWrite(B_1A, 0);
  analogWrite(B_1B, speed);
}

/**
   오른쪽 모터를 정회전, 왼쪽 모터를 정지하여 우회전합니다.
*/
void right() {
  //모터A 정회전
  analogWrite(A_1A, speed);
  analogWrite(A_1B, 0);
  //모터B 정지
  analogWrite(B_1A, 0);
  analogWrite(B_1B, 0);
}

/**
   오른쪽 모터를 정지, 왼쪽모터를 정회전하여 좌회전합니다.
*/
void left() {
  //모터A 정지
  analogWrite(A_1A, 0);
  analogWrite(A_1B, 0);
  //모터B 정회전
  analogWrite(B_1A, speed);
  analogWrite(B_1B, 0);
}

/**
   오른쪽,왼쪽모터를 모두 정지합니다.
*/
void stop() {
  //모터A 정지
  analogWrite(A_1A, 0);
  analogWrite(A_1B, 0);
  //모터B 정지
  analogWrite(B_1A, 0);
  analogWrite(B_1B, 0);
}

/**
   초음파센서를 이용해서 벽까지의 거리(mm)를 구합니다.
*/
float getDistanceCM() {
  digitalWrite(echoPin, LOW);
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  //    거리값을 저장합니다.
  float distance = pulseIn(echoPin, HIGH)  / 29.0 / 2.0;

  return distance;
}

void setup() {
  //핀을 초기화합니다.
  //L298 모터드라이버의 핀들을 출력으로 변경합니다.
  pinMode(A_1A, OUTPUT);
  pinMode(A_1B, OUTPUT);
  pinMode(B_1A, OUTPUT);
  pinMode(B_1B, OUTPUT);
  digitalWrite(A_1A, LOW);
  digitalWrite(A_1B, LOW);
  digitalWrite(B_1A, LOW);
  digitalWrite(B_1B, LOW);

  //초음파센서를 초기화합니다.
  pinMode(trigPin, OUTPUT); // Trigger is an output pin
  pinMode(echoPin, INPUT); // Echo is an input pin

}

void loop() {
  //30cm이내에 벽이 있다면 아래의 코드를 실행합니다.
  if (getDistanceCM() < 30) {
    //1초간 정지합니다.
    stop();
    delay(1000);
    
    //       1초간 후진합니다.
    back();
    delay(1000);
    
    // 오른쪽으로 2초간 운영
    right();
    delay(5000);

  } else {
    forward();
  }
}