#include <Servo.h>
Servo servo;
#define trigPin 2
#define echoPin 3
#define servoPin 9
int distApp = 0;
int pos =0;
int posCin = 0;
int posMetro = 0;
int posBase =0;
void setup() {
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
servo.attach(9);
}
void loop() {
distanza();
if(distApp>250){
posBase=0;
base();
}
else if (distApp>100 && distApp<250){
movServoSensoreUnMetro();
}
else if (distApp<100 && distApp > 40) {
movServoSensoreCin();
}
else {
pos = 0;
movServoSensore();
}
}
void movServoSensore(){
for(pos = 0; pos < 190; pos += 12)
{
servo.write(pos);
delay(7);
}
for(pos = 190; pos>=1; pos-= 12)
{
servo.write(pos);
delay(7);
}
}
//DISTANZA MAGGIORE DI 1 MT E MINORE DI 2,50 MT
void movServoSensoreUnMetro(){
for(posMetro = 0; posMetro < 130; posMetro += 12)
{
servo.write(posMetro);
delay(20);
}
for(posMetro = 130; posMetro>=1; posMetro-= 12)
{
servo.write(posMetro);
delay(20);
}
}
//DISTANZA MINORE UN METRO
void movServoSensoreCin(){
for(posCin = 0; posCin < 70; posCin += 12)
{
servo.write(posCin);
delay(18);
}
for(posCin = 70; posCin>=1; posCin-= 12)
{
servo.write(posCin);
delay(18);
}
}
//DISTANZA BASE CON DISTANZA MAGGIORE DI 2,50 MT (SENZA PERSONE DAVANTI)
void base(){
for(posBase = 0; posBase < 190; posBase += 30)
{
servo.write(posBase);
delay(6);
}
for(posBase = 190; posBase>=1; posBase-= 75)
{
servo.write(posBase);
delay(6);
}
}
void distanza(){
long duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
if (distance >= 300 || distance <= 0){
Serial.println("Fuori");
}
else {
Serial.print(distance);
Serial.println(" cm");
}
distApp = distance;
delay(500);
}