#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 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() {
  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);
  }
  digitalWrite(led_rojo, HIGH);
  Serial.println("LED Rojo On");
  delay(100);
  digitalWrite(led_rojo, LOW);
  //Serial.println("LED Rojo Off");
  delay(100);
  digitalWrite(led_verde, HIGH);
  Serial.println("LED Verde On");
  delay(1000);
  digitalWrite(led_verde, LOW);
  //Serial.println("LED Rojo Off");
  delay(1000);
  digitalWrite(led_amarillo, HIGH);
  Serial.println("LED Amarillo On");
  delay(2000); 
  digitalWrite(led_amarillo, LOW);
  delay(2000); 

  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);
  }
}