Page 110 - MDP2022-3

pinMode(IN4, OUTPUT);
pinMode(A2, INPUT);
servoLeft.attach(12);
servoRight.attach(13);

servoUp.attach(A1);
servoPlus.attach(A0);
}

void loop() {

sw1n = digitalRead(A2); //스위치 상태 확인

digitalWrite(trig1, LOW); //초음파 센서를 초기화 하는 과정입니다.
delayMicroseconds(2);
digitalWrite(trig1, HIGH);
delayMicroseconds(10);

digitalWrite(trig1, LOW);
duration1 = pulseIn(echo1,HIGH); // pulseIn() 함수를 이용한 초음파 센서
주기(ms) 반환
distance1 = duration1 / 29.0 / 2.0; // cm 단위 거리 변환

delay(5);
digitalWrite(trig2, LOW);
delayMicroseconds(2);
digitalWrite(trig2, HIGH);
delayMicroseconds(10);
digitalWrite(trig2, LOW);

duration2 = pulseIn(echo2, HIGH);
distance2 = duration2 / 29.0 / 2.0;

if(distance1 < 4 && distance1 > 1) //왼쪽초음파센서가 감지한 거리가 4CM 이하이고

1CM보다 클 때
{
servoLeft.write(0); // 서보모터 제어
servoRight.write(90); // 서보모터 제어
servoUp.write(90);
delay(10000);

servoUp.write(0);
servoLeft.write(90); // 서보모터 제어
servoRight.write(0); // 서보모터 제어
delay(1000);

}