#include <ESP32Servo.h>

int servo_A_Pin = 18;
int servo_B_Pin = 17;
Servo servo_A;
Servo servo_B;
int pos = 0;

int led_rojo = 23;
int led_amarillo = 22;
int led_verde = 21;

void mueve_servo_A(void);
void mueve_servo_B(void);
void led_ON (int led, int retardo);
void led_OFF(int led, int retardo);


void setup() {
  pinMode(led_rojo, OUTPUT);
  pinMode(led_amarillo, OUTPUT);
  pinMode(led_verde, OUTPUT);
  servo_A.attach(servo_A_Pin, 500, 2400);
  servo_B.attach(servo_B_Pin, 500, 2400);
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
 
}

void loop() {
  mueve_servo_A();
  mueve_servo_B();
  Serial.println("LED Rojo On");
  led_ON(led_rojo, 100);
  Serial.println("LED Rojo Off");
  led_OFF(led_rojo, 100);
  Serial.println("LED Verde On");
  led_ON(led_verde, 1000);
  Serial.println("LED Verde Off");
  led_OFF(led_verde, 1000);
  Serial.println("LED Amarillo On");
  led_ON(led_amarillo, 2000);
  Serial.println("LED Amarillo Off");
  led_OFF(led_amarillo, 2000);
}
void led_ON (int led, int retardo)
{
  digitalWrite(led, HIGH);
  delay(retardo);
}
void led_OFF (int led, int retardo)
{
  digitalWrite(led, LOW);
  delay(retardo);
}

void mueve_servo_A(void)
{
  Serial.println("Servo_A de 0 a 180");
  for (pos = 0; pos <= 180; pos += 1) {
    servo_A.write(pos);
    delay(15);
  }
  Serial.println("Servo_A de 180 a 0");
  for (pos = 180; pos >= 0; pos -= 1) {
    servo_A.write(pos);
    delay(15);
  }
}
void mueve_servo_B(void)
{
  Serial.println("Servo_B de 0 a 180");
  for (pos = 0; pos <= 180; pos += 1) {
    servo_B.write(pos);
    delay(15);
  }
  Serial.println("Servo_B de 180 a 0");
  for (pos = 180; pos >= 0; pos -= 1) {
    servo_B.write(pos);
    delay(15);
  }
}