//Integrantes:

//Carlos Diaz Boucher

//Juan Lopez
//Victor Alejandro
//Jua Torres

// Includes the Servo library

#include <Servo.h>

const int EchoPin = 13;

const int TriggerPin = 8;

Servo myServo0; // Creates a servo object for controlling the servo motor
Servo myServo1; // Creates a servo object for controlling the servo motor
Servo myServo2; // Creates a servo object for controlling the servo motor
int sw=0;
int cm=0;
void setup() {
Serial.begin(9600);
pinMode(TriggerPin, OUTPUT);
pinMode(EchoPin, INPUT);
myServo0.attach(6); // Defines on which pin is the servo motor attached
myServo1.attach(9); // Defines on which pin is the servo motor attached
myServo2.attach(10); // Defines on which pin is the servo motor attached

myServo0.write(0);
delay(1000);
myServo1.write(120);
delay(1000);
myServo2.write(90);
delay(1000);
}

void loop() {
cm = ping(TriggerPin, EchoPin);
Serial.print("Distancia: ");
Serial.println(cm);
if (cm>0 && cm<6 && sw==0){
sw=1;
for(int j=0;j<=90;j++){
myServo0.write(j);
delay(30);
}
//myServo0.write(90);
delay(2000);
}
if (sw==1){
sw=2;
for(int i=120;i>30;i--){
myServo1.write(i);
delay(30);
}
delay(2000);
}

if (sw==2){
sw=3;
for(int i=90;i<=150;i++){
 myServo2.write(i);
delay(30);
}
delay(2000);
}
if (sw==3){
sw=4;
for(int i=150;i>90;i--){
myServo2.write(i);
delay(30);
}
delay(2000);
}
if (sw==4){
sw=5;
for(int i=30;i<120;i++){
myServo1.write(i);
delay(30);
}
delay(2000);
}
if (sw==5){
sw=0;
for(int i=90;i>0;i--){
myServo0.write(i);
delay(30);
}
delay(5000);
}
}

//devuelve la distancia
int ping(int TriggerPin, int EchoPin) {
long duration, distanceCm;

digitalWrite(TriggerPin, LOW); //para generar un pulso limpio ponemos a LOW 4us

delayMicroseconds(4);
digitalWrite(TriggerPin, HIGH); //generamos Trigger (disparo) de 10us
delayMicroseconds(10);
digitalWrite(TriggerPin, LOW);

duration = pulseIn(EchoPin, HIGH); //medimos el tiempo entre pulsos, en

microsegundos

distanceCm = duration * 10 / 292/ 2; //convertimos a distancia, en cm

return distanceCm;
}