/*  Controlling the sweep of a servo using an HC-SR04 distance reading. Map() function
 *  is used to map a distance "window", such as between 20 and 100 inches. The servo
 *  is also mapped to sweep from 0 to 165 degreens within the distance window.
 *  This achieve a smooth linear movement of the servo arm, reacting to both
 *  incoming and outgoing distance change. Servo with not respond to distance readings
 *  outside of the mapped window.
*/

// Library:
#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>

// Wiring: SDA pin to A4, SCL pin to A5
// Hex number is a unique 8bit address used by I2C devices, was 0x3F
// Create an LCD object, "lcd" can be changed to any name
LiquidCrystal_I2C lcd(0x27, 16, 2);

Servo myServo; // myServo is called an object. A maximum of eight servo objects can be created

// Variables for the HC-SR04:
#define trigPin 2
#define echoPin 3

int servoValue; // value from 0 to 180 (sweep area)
#define servoPin 9

int duration; // stores time between send and receive in microseconds
uint8_t distanceInch;

#define redLed 6
uint8_t pwmValue;
uint16_t delayTime = 500;


void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(redLed, OUTPUT);  
  myServo.attach(servoPin); // myServo object attached to servoPin
  lcd.init(); // Initiate LCD
  lcd.backlight();
}

void loop() {
  // Set trigPin low for 5us to get a clear starting signal
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);

  // Set the trigPin high for 10us to generate signal
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the length of the pulse (microseconds) received by echoPin using function pulseIn()
  duration = pulseIn(echoPin, HIGH);
  Serial.print("duration: "); Serial.println(duration);
  distanceInch = (duration / 74) / 2; // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per second)

  pwmValue = map(distanceInch, 0, 27, 255, 0);
  Serial.print("pwmValue: "); Serial.println(pwmValue);
  analogWrite(redLed, pwmValue);
  if (distanceInch > 28) {
    analogWrite(redLed, 0);
  }

  // Print the measurement in the serial console
  Serial.print("distance inch = "); Serial.println(distanceInch);

  // Display on 16x2 LCD
  lcd.clear();
  lcd.setCursor(0, 0); // Set curser to column 0, line 0
  lcd.print("Distance");
   
  lcd.setCursor(0,1);
  lcd.print(distanceInch);
  lcd.print(" inches");

  // Map function maps distance (0" - 30") to 0 - 180 degree sweep of servo
  servoValue = map(distanceInch, 0, 28, 0, 180 );
  if (distanceInch > 28) {
    Serial.print("if servoValue: "); Serial.println(servoValue);
    servoValue = 180;
  }
  Serial.print("servoValue: "); Serial.println(servoValue);
 
  myServo.write(servoValue); // sweeps servo in and out to incoming, outgoing signal


  delay(delayTime);
}