#4.15.5.1. 예제

#define MA1 9

#define MA2 10

#define MB1 5

#define MB2 6

#define ECHO 3

#define TRIG 4

void forward(int a) {

  digitalWrite(MA1, 1);

  digitalWrite(MA2, 0);

  digitalWrite(MB1, 1);

  digitalWrite(MB2, 0);

  delay(a);

}

void backward(int b) {

  digitalWrite(MA1, 0);

  digitalWrite(MA2, 1);

  digitalWrite(MB1, 0);

  digitalWrite(MB2, 1);

  delay(b);

}

void left(int c) {

  digitalWrite(MA1, 0);

  digitalWrite(MA2, 1);

  digitalWrite(MB1, 1);

  digitalWrite(MB2, 0);

  delay(c);

}

void setup() {

  pinMode(MA1, OUTPUT);

  pinMode(MA2, OUTPUT);

  pinMode(MB1, OUTPUT);

  pinMode(MB2, OUTPUT);

  pinMode(ECHO, INPUT);

  pinMode(TRIG, OUTPUT);

  Serial.begin(9600);

}

void loop() {

  digitalWrite(TRIG, LOW);

  delayMicroseconds(2);

  digitalWrite(TRIG, HIGH);

  delayMicroseconds(10);

  digitalWrite(TRIG, LOW);

  long duration = pulseIn(ECHO, HIGH);

  long distance = duration / 58.2;

  Serial.println(distance);

  if (distance < 10) {

    backward(1000);

    left(1000);

  } else {

    forward(100);

  }

}