#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h> //https://github.com/johnrickman/LiquidCrystal_I2C/

Servo myservo;
LiquidCrystal_I2C lcd(0x27, 16, 2);

float DistanceSec = 20;
int pos = 0;
const int echoPin     = A0;
const int trigPin     = A1;
const int cameraPin   = 10;
const int motorPin    = 11;
const int ledPin      = 12;
const int buzzerPin   = 13;
float distanceCm, duration;

void setup() {
  myservo.attach(motorPin);
  lcd.init();
  lcd.init();
  lcd.backlight();
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(cameraPin, OUTPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);
  digitalWrite(cameraPin, LOW);
  digitalWrite(ledPin, LOW);
  digitalWrite(buzzerPin, LOW);
}

void loop() {
  for (pos = 0; pos <= 180; pos += 1) {
    myservo.write(pos);
    ReadUltrasonicSensor();
    ShowDataOnLCD();
    AlertCondition();
    delay(80);
  }
  for (pos = 180; pos >= 0; pos -= 1) {
    myservo.write(pos);
    ReadUltrasonicSensor();
    ShowDataOnLCD();
    AlertCondition();
    delay(80);
  }
}

void ReadUltrasonicSensor() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); //envoyer une impulsion de 10 micro seconds
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distanceCm = duration * 0.034 / 2;
}

void ShowDataOnLCD() {
  lcd.setCursor(0, 0);
  lcd.print("Distance:");
  lcd.print(distanceCm);
  lcd.print("cm ");
  delay(10);
  lcd.setCursor(0, 1);
  lcd.print("Angle : ");
  lcd.print(pos);
  lcd.print(" deg ");
}

void AlertCondition() {
  if (distanceCm <= DistanceSec) {
    digitalWrite(cameraPin, HIGH);
    digitalWrite(ledPin, HIGH);
    digitalWrite(buzzerPin, HIGH);
    delay(5000);
    digitalWrite(ledPin, LOW);
    digitalWrite(buzzerPin, LOW);
    digitalWrite(cameraPin, LOW);
    delay(2000);
  }
  else {
    digitalWrite(ledPin, LOW);
    digitalWrite(buzzerPin, LOW);
    digitalWrite(cameraPin, LOW);
  }
}