#include <Servo.h>
Servo M1;
Servo M2;
#define WIFI1 2 //modulo WiFI entrada
#define WIFI2 3 //modulo WiFI entrada

unsigned long srv1Go = 0;
unsigned long srv2Go = 0;
unsigned long srv1Stay = 0;
unsigned long srv2Stay = 0;
int stepTime = 20;
int returnTime = 5000;
int targetPosition1 = 90; // Posição desejada do servo
int targetPosition2 = 90; // Posição desejada do servo
int pos1 = 0;
int pos2 = 0;
bool flag1 = false;
bool flag2 = false;

void setup() {
  M1.attach(4, 500, 2500);
  M2.attach(5, 500, 2500);
  pinMode(WIFI1, INPUT);
  pinMode(WIFI2, INPUT);
  M1.write(0); // Posicione o servo em 0 graus no início
  M2.write(0); // Posicione o servo em 0 graus no início
  srv1Go = millis();
  srv2Go = millis();
}

void loop() {
  if (digitalRead(WIFI1) == HIGH) {
    flag1 = true;
  }
  if (digitalRead(WIFI2) == HIGH) {
    flag2 = true;
  }

  // Mova o servo gradualmente em direção à posição desejada
  if (flag1 == true) {
    if (pos1 <= targetPosition1) {
      M1.write(pos1);
      if (millis() - srv1Go > stepTime) {
        pos1++;
        srv1Go = millis();
        srv1Stay = millis();
      }
    }
    else {
      flag1 = false;
      pos1 = 0;
    }
  }
  else {
    if (millis() - srv1Stay > returnTime) {
      M1.write(0);
    }
  }

  // Mova o servo gradualmente em direção à posição desejada
  if (flag2 == true) {
    if (pos2 <= targetPosition2) {
      M2.write(pos2);
      if (millis() - srv2Go > stepTime) {
        pos2++;
        srv2Go = millis();
        srv2Stay = millis();
      }
    }
    else {
      flag2 = false;
      pos2 = 0;
    }
  }
  else {
    if (millis() - srv2Stay > returnTime) {
      M2.write(0);
    }
  }
}