/****************************************************************************************/
/****************************************************************************************/
/*****                  PONTIFICIA UNIVERSIDAD CATÓLICA DEL PERÚ                    *****/
/*****                     FACULTAD DE CIENCIAS E INGENIERÍA                        *****/
/*****                           SISTEMAS DIGITALES B                               *****/
/****************************************************************************************/
/*****          Tema:               PWM                                             *****/
/*****          Proyecto:           PWM_Servo                                       *****/
/****************************************************************************************/
/*****          EvalBoard:          ESP32 S3 DevKitC C1                             *****/
/*****          Autor:              Rolando Sánchez Ponce                           *****/
/*****          Fecha:              Octubre 2024                                    *****/
/****************************************************************************************/
/***** Enunciado:                                                                   *****/
/***** Tenemos conectado al módulo ESP32-S3 dos servomotores un potenciómetro y un  *****/
/***** pulsador. El primer servomotor es controlado por el potenciómetro, el eje va *****/
/***** entre 0 y 180 grados dependiendo de la posición del potenciómetro. El segun- *****/
/***** do servomotor es controlado por el pulsador, cada vez que se presiona posi-  *****/
/***** ciona el eje en 180grados durante 500ms y luego regresa a los 90 grados.     *****/
/****************************************************************************************/
/****************************************************************************************/

// Definiciones para pines y constantes
#define pinServo1 41
#define pinServo2 19
#define pinPOT 9
#define pinPUL 11
#define res12bits 12  // Resolución de 12 bits
#define frecPWM 50    // Frcuencia PWM de 50Hz

int angulo, cicloTrabajoServo1, cicloTrabajoServo2;

void setup() {
  // Configuramos pines
  pinMode(pinPUL, INPUT_PULLUP);
  pinMode(pinPOT, INPUT);
  // Inicializamos pines PWM
  analogWrite(pinServo1, 0);
  analogWrite(pinServo2, 0);
  // PWM para ambos servomotores 50Hz y resolución de 12bits
  analogWriteResolution(pinServo1, res12bits);
  analogWriteFrequency(pinServo1, frecPWM);
  analogWriteResolution(pinServo2, res12bits);
  analogWriteFrequency(pinServo2, frecPWM);
  // Servomotor 2 inicializa en 90 grados
  cicloTrabajoServo2 = angulo_a_cicloTrabajo(90);
  analogWrite(pinServo2, cicloTrabajoServo2);  
}

void loop() {
  // Leemos potenciómetro y actualizamos posición de servomotor 1
  angulo = map(analogRead(pinPOT), 0, 4095, 0, 180);
  cicloTrabajoServo1 = angulo_a_cicloTrabajo(angulo);
  analogWrite(pinServo1, cicloTrabajoServo1);
  
  // Si se presiona el pulsador, movimiento de servomotor 2
  if(digitalRead(pinPUL)==LOW){
    delay(5);
    while(digitalRead(pinPUL)==LOW);
    delay(5);

    cicloTrabajoServo2 = angulo_a_cicloTrabajo(180);
    analogWrite(pinServo2, cicloTrabajoServo2);
    delay(500);
    cicloTrabajoServo2 = angulo_a_cicloTrabajo(90);
    analogWrite(pinServo2, cicloTrabajoServo2);
  }

  delay(10); // this speeds up the simulation
}

// Convierte el ángulo a ciclo de trabajo con una resolución de 12bits
int angulo_a_cicloTrabajo(int angulo180){
  int cicloTrabajo4095;
  if(angulo>180) angulo=180; 
  if(angulo<0) angulo=0;
  cicloTrabajo4095 = map(angulo180, 0, 180, 123, 491);
  return cicloTrabajo4095;
}