// Jose Edson-Robotics Arms:
#include <WiFi.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <time.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C LCD = LiquidCrystal_I2C(0x27, 16, 2);
#include <ESP32Servo.h>
const int buttonPin = 12; 
const int SRV1Pin = 33;
const int SRV2Pin = 32;
const int SRV3Pin = 25;
const int SRV4Pin = 26;
const int SRV5Pin = 27;
const int SRV6Pin = 14;
Servo SRV1;
Servo SRV2;
Servo SRV3;
Servo SRV4;
Servo SRV5;
Servo SRV6;

int pos0 = 0;
int pos1 = 45;
int pos2 = 90;
int pos3 = 135;
int pos4 = 180;
int trigPin = 19;
int echoPin = 4;
int pos_sd= 13;


// Pin definitions
#define SRV1_Pin 33 //rotate arm
#define SRV2_Pin 32 //arm lift step1
#define SRV3_Pin 25 //arm lift step2
#define SRV4_Pin 26 //arm lift step3
#define SRV5_Pin 27 //rotates clamp
#define SRV6_Pin 14 //open and close the clamp
#define ECHO_PIN 4  // Ultrasonic sensor echo pin
#define TRIG_PIN 19 // Ultrasonic sensor trigger pin


void setup() {

  Serial.begin(115200);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  LCD.init();
  LCD.backlight();
  LCD.setCursor(0, 0);
  LCD.print("Conectando");
  LCD.setCursor(0, 1);
  LCD.print("WiFi");
  delay(600);
  LCD.print(("JoseEdson_PW"));
  delay(1000);

   
  SRV1.write(90);
  delay(600);
  
  SRV1.write(180);
  delay(300);
 
  SRV2.write(90);
  delay(600);
  
  SRV2.write(180);
  delay(300);

  //WiFi.begin("user", "password", 6);
  //while (WiFi.status() != WL_CONNECTED) {
    //Serial.println(("Aguardando"));
    //delay(500);
  //}

  //Serial.println("number: ");
  //Serial.println("WiFi conectado");
  //Serial.print("IP: ");
  //Serial.println(WiFi.localIP());
  
  //LCD.clear();
  //LCD.setCursor(0, 0);
  //LCD.print("Online");
  //LCD.setCursor(0, 1);
  //LCD.print("Atualizando...");

 }

void loop() {
  
  if (distance >= 300) {
    while (distance > 20) {
      Serial.println("Can bom nuoc vao be !");
      Serial.print("Khoang cach: ");
      Serial.println(distance);
      Serial.println("Dang bom nuoc !");
      for (int g = 0; g <= 360; g++) {
        servo1.write(g);
        delay(20);
        pumpState = 1;
        readSensors();
      }
 }