|
#include Servo myservo1;
Servo myservo2;//서보모터 객체 생성 int pos = 0;
int pos2 = 0:
int echoPin = 7;
int trigPin = 8;
//초음파 센서의 핀번호를 설정한다.
float duration;
float distance = 100; void setup() {
myservo1.attach(9);
myservo2.attach(11);//서보모터 핀번호를 9번으로 설정
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
// trig를 출력모드로 설정, echo를 입력모드로 설정
pinMode(10, OUTPUT); // LED
} void loop() {
digitalWrite(trigPin, HIGH);
delay(10);
digitalWrite(trigPin, LOW);
// 초음파를 보낸다. 다 보내면 echo가 HIGH 상태로 대기하게 된다. duration = pulseIn(echoPin, HIGH); // echoPin 이 HIGH를 유지한 시간을 저장 한다.
distance = ((float)(340 * duration) / 10000) / 2; // HIGH 였을 때 시간(초음파가 보냈다가 다시 들어온 시간)을 가지고 거리를 계산 한다. //초음파 센서로 측정한 거리가 20cm미만일 경우(쓰레기통에 근접하는 물체가 감지 될 경우)
if (distance < 20) { digitalWrite(10, HIGH);
//쓰레기통의 문을 연다
for (pos = 10; pos < 100; pos += 2)
for (pos2 = 10; pos < 100; pos +=2)
{
myservo1.write(pos);
myservo2.write(pos2);
delay(10);
}
delay(1000); //3초의 지연시간을 주고 난 뒤 쓰레기통의 문을 닫는다
for (pos = 100; pos >= 10; pos -= 2)
for (pos2 = 100; pos >= 10; pos -= 2)
{
digitalWrite(10, LOW);
myservo1.write(pos);
myservo2.write(pos2);
delay(10);
}
}
delay(50);
}
|
