//#include <HCSR04.h>
#include <Servo.h>
Servo s1;                                         //monster mechanism servo
Servo s2;                                         //noise mechanism servo
int led_pin = 8;
int disMax = 150;                                 //max working distance set to 150cm
int angle1 = 180;                                  //servo 1 movement range
int angle2 = 20;                                  //servo 2 movement range
//UltraSonicDistanceSensor distanceSensor(5, 3);


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  s1.attach(6);
  s2.attach(7);
  pinMode(led_pin, OUTPUT);

}

void loop() {

  //int dist = distanceSensor.measureDistanceCm();  // We define the variable "dist" as the distance detected by the ultrasonic sensor
  //Serial.println(dist);                           // distance verification using serial monitor
  //delay(500);

  //if ( dist < disMax && dist > 0) {   // We start the Boolean function "if". If the distance is lower than the max distance (cm) and has a positive value the function runs.

    delay (1000);                    // mechanism waits 1s before starting its functions

    digitalWrite(led_pin, HIGH);     // LEDs set to ON
    s2.write(angle2);                // servo 2 turns 20º
    delay(500);                      // turns for half a second
    s2.write(-angle2);               // servo 2 turns 20º the other way
    delay(500);                      // turns for half a second

    s1.write(angle1);                // servo 1 turns 180º (the monster goes up)
    delay (5000);                    // mechanism waits 5s before
    s1.write(-angle1);               // servo 1 turns 180º the other way (the monster goes down)
    delay(5000);
    digitalWrite(led_pin, LOW);      //  LEDs set to OFF
  //}

}