#include <ESP32Servo.h>


Servo servo[2];
byte servo_pins[2] = {25, 33};

// prototipos de função
void Calibrar(), LimparBuffer();

void setup()
{
  Serial.begin(9600);

  servo[0].attach(servo_pins[0]);
  servo[1].attach(servo_pins[1]);

  for (byte i = 0; i < /*nº de servos*/ sizeof(servo_pins)/sizeof(byte); i++)
  {
    servo[i].attach(servo_pins[i]);
  }
}

void loop()
{
  Calibrar();
}

void Calibrar()
{
  if (Serial.available() > 0 && Serial.read() == 'c')
  {

    LimparBuffer();

    //# Selecionar eixo
    Serial.println("Selecione um servo:");

    while (Serial.available() == 0) delay(50);

    byte axis = Serial.parseInt();
    Serial.println("Servo " + String(axis) + " selecionado");

    axis--;

    LimparBuffer();

    Serial.println("Digite o ângulo do servo");

    while (Serial.available() == 0) delay(50);

    //# mover servo
    byte ang = Serial.parseInt();
    servo[axis].write(ang);

    Serial.println("Angulo = " + String(servo[axis].read()));
  }
}

void LimparBuffer()
{
  while (Serial.available() > 0)
  {
    Serial.read();
  }
}