// Jose Edson-Robotics Arms:

#define TRIG_PIN 19
#define ECHO_PIN 4 
#include <time.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C LCD = LiquidCrystal_I2C(0x27, 16, 2);
#include <ESP32Servo.h>

int colunas = 16;
int linhas = 2;

int cm = 0;
int led1 = 12;
int led2 = 2;
int led3 = 15;
int freq = 440;
int buzzer = 35;


Servo servo_33;
Servo servo_32;
Servo servo_25;
Servo servo_26;
Servo servo_27;
Servo servo_14;
int pos = 0;

// Pin definitions
#define servo1Pin=33 //rotate arm
#define servo2Pin=32 //arm lift step1
#define servo3Pin=25 //arm lift step2
#define servo4Pin=26 //arm lift step3
#define servo5Pin=27 //rotates clamp
#define servo6Pin=14 //open and close the clamp
#define ECHO_PIN 4  // Ultrasonic sensor echo pin
#define TRIG_PIN 19 // Ultrasonic sensor trigger pin

const int trigpin = 19;
const int echopin = 4;
long duracao;
int distancia;


void setup() 
{
  servo_33.attach(33);
  servo_32.attach(32);
  servo_25.attach(25);
  servo_26.attach(26);
  servo_27.attach(27);
  servo_14.attach(14);

  pinMode (led1, OUTPUT);
  pinMode (led2, OUTPUT);
  pinMode (led3, OUTPUT);
  pinMode(trigpin, OUTPUT);
  pinMode(echopin, INPUT);
  
  pinMode(buzzer, OUTPUT);

  Serial.begin(115200);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT); 
  
  
  LCD.init();
  LCD.backlight();
  
  }

void loop() 
{
  digitalWrite(trigpin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigpin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigpin, LOW);
  duracao = pulseIn(echopin, HIGH);
  distancia = duracao * 0.03408/2;
  int x = map(distancia,0,400,0,180);
  
  Serial.print(distancia);
  Serial.println(" CM");
  LCD.setCursor(0, 0);
  LCD.print("Distance: ");
  LCD.setCursor(10, 0);
  LCD.print(distancia);
  LCD.setCursor(14, 0);
  LCD.print("cm");
  LCD.setCursor(0, 1);
  LCD.print("By Eng JoseEdson");

  
   servo_33.write(45);
   delay(1000);
   
   servo_32.write(45);
   delay(1000);

   servo_25.write(45);
   delay(1000);

   servo_26.write(45);
   delay(1000);

   servo_33.write(45);
   digitalWrite(led1, 0);
   digitalWrite(led2, 1);
   digitalWrite(led3, 0);
   delay(1000);

   servo_32.write(45);
   delay(1000);

   servo_25.write(45);
   delay(1000);

   servo_26.write(-180);
   digitalWrite(led1, 0);
   digitalWrite(led2, 0);
   digitalWrite(led3, 1);
   delay(1000);

  servo_33.write(-90);
   delay(1000);

  //troque a posição do servo motor de 0º à 180º no 1º passo
  for (pos = 0; pos <= 180; pos += 1) {
  servo_27.write(pos) ;
  //aguarde 15 ms para o servo voltar ao inicio
  delay(15) ;
}
  //troque a posição do servo motor de 180º à 0º no 1º passo
  for (pos = 180; pos >= 0; pos -= 1) {
  servo_27.write(pos) ;
  //aguarde 15 ms para o servo voltar ao inicio
  delay(15) ;
  }
     
   //troque a posição do servo motor de 0º à 180º no 1º passo
  for (pos = 0; pos <= 180; pos += 1) {
  servo_14.write(pos) ;
  //aguarde 15 ms para o servo voltar ao inicio
  delay(15) ;
}
  //troque a posição do servo motor de 1800º à 0º no 1º passo
  for (pos = 180; pos >= 0; pos -= 1) {
  servo_14.write(pos) ;
   digitalWrite(led1, 1);
   digitalWrite(led2, 0);
   digitalWrite(led3, 0);
  //aguarde 15 ms para o servo voltar ao inicio
  delay(15) ;
}

    
  if(cm <= 13){
    servo_14.write(45);
   digitalWrite(led1, 0);
   digitalWrite(led2, 0);
   digitalWrite(led3, 1);
   delay(1000);
   }

   Serial.print("Distancia: ");
   Serial.print(cm);
   Serial.println("cm");

   if((cm > 3)&&(cm <=15)){
    tone(buzzer,2*freq,200);
    delay(200);
    delay(200);
  }
  if((cm > 16)&&(cm <=35)){
    tone(buzzer,660,400);
    delay(400);
    delay(400);
  }
  if((cm > 36)&&(cm<=60)){
    tone(buzzer,freq,800);
    delay(800);
    delay(800);
  }

  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);
  float distance = (duration * 0.034) / 2; // Convert to cm

  Serial.println(distance); // Send distance to Processing
  delay(100); // Update rate

  }