(SAL) Sonar&Alexa Lights

/* HC-SR04 Sensor

   https://www.dealextreme.com/p/hc-sr04-ultrasonic-sensor-distance-measuring-module-133696

  

   This sketch reads a HC-SR04 ultrasonic rangefinder and returns the

   distance to the closest object in range. To do this, it sends a pulse

   to the sensor to initiate a reading, then listens for a pulse 

   to return.  The length of the returning pulse is proportional to 

   the distance of the object from the sensor.

     

   The circuit:

 * VCC connection of the sensor attached to +5V

  * GND connection of the sensor attached to ground

  * TRIG connection of the sensor attached to digital pin 2

  * ECHO connection of the sensor attached to digital pin 4





   Original code for Ping))) example was created by David A. Mellis

   Adapted for HC-SR04 by Tautvidas Sipavicius



   This example code is in the public domain.

 */


#include <Servo.h>
Servo myservo;  // create servo object to control a servo
Servo myservo2;

const int trigPin = 4;

const int echoPin = 2;
const int servoMin= 7;
const int servo2Min = 2;
const int servoMax = 55;
const int servo2Max = 32;

const long timerOnMax = 50;
const long tol = 1;
const long distMax = 30;
const long distMin = 5;
long timerOn = 0;
long prev = -999;
int pos;
void setup() {

  // initialize serial communication:
  myservo2.attach(10);
  myservo.attach(11);
  Serial.begin(9600);

  myservo.write(servoMax);              
  myservo2.write(servo2Max);
  Serial.println("Start - Off");
}



 
void loop()
{
 int input = Serial.read();
 if (input == 'A') { 
     for (pos = servoMax; pos >= servoMin; pos -= 3) { // goes from 180 degrees to 0 degrees
        myservo.write(pos);              // tell servo to go to position in variable 'pos'
        delay(200);
     }                       // waits 15ms for the servo to reach the position 
 } else if(input == 'B') { 
    for (pos = servoMin; pos <= servoMax; pos += 3) { // goes from 0 degrees to 180 degrees
        // in steps of 1 degree
        myservo.write(pos);
        delay(200);
    }
 } else if (input == 'C') { 
     for (pos = servo2Max; pos >= servo2Min; pos -= 3) { // goes from 180 degrees to 0 degrees
        // tell servo to go to position in variable 'pos'
        myservo2.write(pos);
        delay(200);
     }                       // waits 15ms for the servo to reach the position 
 } else if(input == 'D') { 
    for (pos = servo2Min; pos <= servo2Max; pos += 3) { // goes from 0 degrees to 180 degrees
        // in steps of 1 degree
        myservo2.write(pos);
        delay(200);
    }
 }  

  // establish variables for duration of the ping, 

  // and the distance result in inches and centimeters:

  long dist;



  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.

  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:

  pinMode(trigPin, OUTPUT);

  digitalWrite(trigPin, LOW);

  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);



  // Read the signal from the sensor: a HIGH pulse whose

  // duration is the time (in microseconds) from the sending

  // of the ping to the reception of its echo off of an object.

   pinMode(echoPin, INPUT);

  // convert the time into a distance
  dist = microsecondsToInches(pulseIn(echoPin, HIGH));

  Serial.print(dist);
  Serial.print(" in.  Timer: ");
  Serial.println(timerOn);
  delay(200);



  if((dist > distMax + tol) || (dist < distMin - tol))
    prev = -999;
  else if(abs(dist - prev) > tol)
  {
    if(timerOn == 0)
    {
      //turnLightOn
      Serial.println("Turn On");
      for (pos = servoMax; pos >= servoMin; pos -= 3) { // goes from 180 degrees to 0 degrees
        myservo.write(pos);              // tell servo to go to position in variable 'pos'
        //myservo2.write(((servo2Min - servoMin)+pos));
        delay(200);                       // waits 15ms for the servo to reach the position
      }
    }
    timerOn = timerOnMax;
    prev = dist;
  }
  if(timerOn > 0)
  {
    timerOn--;
    if(timerOn == 0)
    {
      //turnLightOff
      Serial.println("Turn Off");
      for (pos = servoMin; pos <= servoMax; pos += 3) { // goes from 0 degrees to 180 degrees
        // in steps of 1 degree
        myservo.write(pos);              // tell servo to go to position in variable 'pos'
        //myservo2.write((servo2Max - pos));
        delay(200);                       // waits 15ms for the servo to reach the position
      }
    }
  }
}



long microsecondsToInches(long microseconds)

{

  // According to Parallax's datasheet for the PING))), there are

  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per

  // second).  This gives the distance travelled by the ping, outbound

  // and return, so we divide by 2 to get the distance of the obstacle.

  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf

  return microseconds / 74 / 2;

}



long microsecondsToCentimeters(long microseconds)

{

  // The speed of sound is 340 m/s or 29 microseconds per centimeter.

  // The ping travels out and back, so to find the distance of the

  // object we take half of the distance travelled.

  return microseconds / 29 / 2;

}