#include <Servo.h>
#define trigPin1 3
#define echoPin1 2
#define trigPin2 5
#define echoPin2 4
#define trigPin3 7
#define echoPin3 6
#define trigPin4 9
#define echoPin4 8
Servo servo; // create servo object to control a servo
int pos = 0; // variable to store the servo position
void setup() {
servo.attach(10);
Serial.begin (9600);
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
pinMode(trigPin4, OUTPUT);
pinMode(echoPin4, INPUT);
pinMode(A0,OUTPUT);
pinMode(A1,OUTPUT);
pinMode(A2,OUTPUT);
pinMode(A3,OUTPUT);
}
void loop() {
long duration1, distance1;
digitalWrite(trigPin1, LOW);
delayMicroseconds(2);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
duration1 = pulseIn(echoPin1, HIGH);
distance1 = (duration1/2) / 29.1;
Serial.print(distance1);
Serial.println(" cm");
if (distance1<40){
floor_1();
}
long duration2, distance2;
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH);
distance2 = (duration2/2) / 29.1;
Serial.print(distance2);
Serial.println(" cm");
if (distance2<40){
floor_2();
}
long duration3, distance3;
digitalWrite(trigPin3, LOW);
delayMicroseconds(2);
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin3, LOW);
duration3 = pulseIn(echoPin3, HIGH);
distance3 = (duration3/2) / 29.1;
Serial.print(distance3);
Serial.println(" cm");
if (distance3<40){
floor_3();
}
long duration4, distance4;
digitalWrite(trigPin4, LOW);
delayMicroseconds(2);
digitalWrite(trigPin4, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin4, LOW);
duration1 = pulseIn(echoPin4, HIGH);
distance4 = (duration4/2) / 29.1;
Serial.print(distance4);
Serial.println(" cm");
if (distance4<40){
floor_4();
}
}
void floor_1(){
servo.write(0);
digitalWrite(A0,HIGH);
delay(10);
digitalWrite(A0,LOW);
}
void floor_2(){
servo.write(60);
digitalWrite(A1,HIGH);
delay(10);
digitalWrite(A1,LOW);
}
void floor_3(){
servo.write(120);
digitalWrite(A2,HIGH);
delay(10);
digitalWrite(A2,LOW);
}
void floor_4(){
servo.write(180);
digitalWrite(A3,HIGH);
delay(10);
digitalWrite(A3,LOW);
}
Comments