/*
 * Fuente original de la biblioteca del módulo PCA9685: https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library
 * 
 * Este es el código de Arduino para el controlador de servo PCA9685 de 32 canales para controlar 32 motores servo a través de WiFi utilizando el MCU ESP32. Obtén este código y el cableado de este video: http://robojax.com/RJT365
 * 
 * Mira el video para este código: https://youtu.be/bvqfv-FrrLM
 * 
 * Videos relacionados
 * Video V4 de PCA9685 32 servo con ESP32: https://youtu.be/JFdXB8Za5Os
 * Video V3 de PCA9685 sobre cómo controlar 32 motores servo: https://youtu.be/6P21wG7N6t4
 * Video V2 de PCA9685 3 formas diferentes de controlar motores servo: https://youtu.be/bal2STaoQ1M
 * Video V1 introducción al PCA9685 para controlar 16 servo: https://youtu.be/y8X9X10Tn1k
 * 
 * Escrito por Ahmad Shamshiri para el canal de videos Robojax www.Robojax.com
 * Fecha: 17 de febrero de 2020, en Ajax, Ontario, Canadá
 * 
 * o haz una donación utilizando PayPal http://robojax.com/L/?id=64
 * 
 * Este código es "COMO ESTÁ" sin garantía ni responsabilidad. Libre de ser utilizado siempre que mantengas esta nota intacta.*
 * Este código ha sido descargado de Robojax.com
 * Este programa es software libre: puedes redistribuirlo y/o modificarlo bajo los términos de la Licencia Pública General de GNU según lo publicado por la Fundación de Software Libre, ya sea la versión 3 de la Licencia, o (a tu elección) cualquier versión posterior.
 * 
 * Este programa se distribuye con la esperanza de que sea útil, pero SIN NINGUNA GARANTÍA; ni siquiera la garantía implícita de COMERCIALIZABILIDAD o ADECUACIÓN PARA UN PROPÓSITO PARTICULAR. Consulta la Licencia Pública General de GNU para más detalles.
 * 
 * Deberías haber recibido una copia de la Licencia Pública General de GNU junto con este programa. Si no, consulta <https://www.gnu.org/licenses/>.
 * / 
 * ////////////////////// Configuraciones de PCA9685 iniciadas
 */
#include <Wire.h>

#include <Adafruit_PWMServoDriver.h>

 // llamada así, utiliza la dirección predeterminada 0x40
Adafruit_PWMServoDriver board1 = Adafruit_PWMServoDriver(0x40);
Adafruit_PWMServoDriver board2 = Adafruit_PWMServoDriver(0x41);
int maximumServo = 32; // cuántos servos están conectados

 // Dependiendo de la marca de su servomotor, el ancho de pulso mínimo y máximo puede variar, usted
 // quiero que estos sean lo más pequeños/grandes posibles sin llegar al límite máximo
 // para el rango máximo. Tendrás que ajustarlos según sea necesario para que coincidan con los servos que
 // ¡ten!
 // Mira el video V1 para entender las dos líneas a continuación: http://youtu.be/y8X9X10Tn1k
#define SERVOMIN  125 // este es el conteo de longitud de pulso 'mínima' (de 4096)
#define SERVOMAX  575 // este es el conteo de longitud de pulso 'máxima' (de un total de 4096)

int servoAngle =0;
int servoStep = 10;

int stepDelay = 50; // 50 milisegundos
int servoAngleMin =0;
int servoAngleMax = 180;

 // ángulo mínimo de cada servomotor
int allServoMin[]={
      0,    0,    0,    0,    0,    0,    0,    0, // 1 a 8
      0,    0,    0,    0,    0,    0,    0,    0, // 9 a 16
      0,    0,    0,    0,    0,    0,    0,    0, // 17 a 24
      0,    0,    0,    0,    0,    0,    0,    0}; // 25 a 32

 // valor máximo de cada servo
int allServoMax[]={
      180,    180,    180,    180,    180,    180,    180,    180, // 1 a 8
      180,    180,    180,    180,    180,    180,    180,    180, // 9 a 16
      180,    180,    180,    180,    180,    180,    180,    180, // 17 a 24
      180,    180,    180,    180,    180,    180,    180,    180}; // 25 a 32

 // posición inicial de los servos
int allServoPosition[] ={
      0,    0,    0,    0,    0,    0,    0,    0, // 1 a 8
      0,    0,    0,    0,    0,    0,    0,    0, // 9 a 16
      0,    0,    0,    0,    0,    0,    0,    0, // 17 a 24
      0,    0,    0,    0,    0,    0,    0,    0}; // 25 a 32

int servoNumber = 100; // servo para moverse
int buttonPushed =0;
int allServo =0;

void handleServo(); // este es un prototipo de la función definida al final de este código
int angleToPulse(int ang); // este es un prototipo de la función definida al final de este código
 // //////////////////////PCA9685 finalizado

#include "PCA9684_32Servo_ESP32.h"

#include <WiFi.h>
#include <WiFiClient.h>
#include <WebServer.h>
#include <ESPmDNS.h>

const char *ssid = "Robojax";
const char *password = "YouTube2020";

WebServer server(80);

const int led = 13;



 // ///////////////////////////////////
void handleRoot() {

String HTML_page = pageHeader_p1;

 if(allServo)
 {
 HTML_page.concat("<div class=\"btn\"><a class=\"angleButton colorAll\"  href=\"/servo?do=stop\">Stop Servo</a></div>");

 }else{
 HTML_page.concat("<div class=\"btn\"><a class=\"angleButton colorAll\"  href=\"/servo?do=all\">All Servo</a></div>");
 }

 for (int i=0; i < maximumServo; i++)
 {

  HTML_page.concat("<div class=\"btn\"><a class=\"angleButton colorBtn\"  href=\"/servo?move=");
  HTML_page.concat(i);
  HTML_page.concat("\">SRV ");
  HTML_page.concat(i+1);
  HTML_page.concat(" </a></div>");
 }

 HTML_page.concat("</body>\n</html>");
 server.send(200, "text/html", HTML_page);
}

void handleNotFound() {
  digitalWrite(led, 1);
  String message = "File Not Found\n\n";
  message += "URI: ";
  message += server.uri();
  message += "\nMethod: ";
  message += (server.method() == HTTP_GET) ? "GET" : "POST";
  message += "\nArguments: ";
  message += server.args();
  message += "\n";

  for (uint8_t i = 0; i < server.args(); i++) {
    message += " " + server.argName(i) + ": " + server.arg(i) + "\n";
  }

  server.send(404, "text/plain", message);
  digitalWrite(led, 0);
}


void setup() {
  board1.begin();
  board2.begin();
  board1.setPWMFreq(60); // Los servos analógicos funcionan con actualizaciones de ~60 Hz.
  board2.setPWMFreq(60);
 // posición inicial de todos los servos
  for(int i=0; i < maximumServo; i++) {
    if(i < 16)
    {
      board1.setPWM(i, 0, angleToPulse(allServoPosition[i]) );
    }else{
      board2.setPWM(i-15, 0, angleToPulse(allServoPosition[i]) );
    }
  } // para fin

    Serial.begin(115200);
    Serial.println("32 channel Servo test!");


 // Control de servos utilizando ESP32 de Robojax.com

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  Serial.println("");


 // Esperar conexión
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.print("Connected to ");
  Serial.println(ssid);
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());


  if (MDNS.begin("robojaxESP32")) {
    Serial.print("MDNS responder started at http: // ");
    Serial.println("robojaxESP32");
  }

  server.on("/", handleRoot);
  server.on("/servo", HTTP_GET, handleServo);

  server.onNotFound(handleNotFound);
  server.begin();
  Serial.println("HTTP server started");
}

void loop() {
	server.handleClient();
  if(allServo ){

    for( int angle =servoAngleMin; angle <= servoAngleMax; angle +=servoStep){
      for(int i=0; i<16; i++)
        {

            board2.setPWM(i, 0, angleToPulse(angle) );
            board1.setPWM(i, 0, angleToPulse(angle) );
        }
        delay(stepDelay);
    }

 // robojax PCA9865 control de servos de 32 canales
  delay(100);

  } // si se presiona
  if(false){
  Serial.print("Servo #");
  Serial.print (servoNumber);
  Serial.print(" Angle ");
  Serial.println(allServoPosition[servoNumber]);
  }


  if( buttonPushed && (servoNumber >=0 && servoNumber < maximumServo) ){
    if(servoNumber < 16)
    {
      board1.setPWM(servoNumber, 0, angleToPulse(allServoPosition[servoNumber]) );
    }else{
      board2.setPWM(servoNumber-15, 0, angleToPulse(allServoPosition[servoNumber]) );
    }
  }

   buttonPushed =0;
}



/*
 * handleServo()
 * actualizar la variable buttonPushed
 * no devuelve nada
 * Escrito por Ahmad Shamshiri el 29 de diciembre de 2019
 * www.Robojax.com
 * http://youTube.com/robojaxTV
 */
void handleServo() {
  if(server.arg("do") == "all" )
  {
    allServo =1;

  }else{
    allServo =0;
  }
   int servoNumberRequested= server.arg("move").toInt();

  if(servoNumberRequested >=0 && servoNumberRequested < maximumServo)
  {

    buttonPushed = 1;
    if(allServoPosition[servoNumberRequested] == allServoMin[servoNumberRequested] ) {
     allServoPosition[servoNumberRequested] = allServoMax[servoNumberRequested];
    }else{
     allServoPosition[servoNumberRequested] = allServoMin[servoNumberRequested];
    }
    servoNumber =servoNumberRequested;
  }

  handleRoot();
} // handleServo() fin


/*
 * angleToPulse(int ang)  
 * obtiene el ángulo en grados y devuelve el ancho del pulso  
 * también imprime el valor en el monitor serie  
 * escrito por Ahmad Shamshiri para Robojax, Robojax.com
 */
int angleToPulse(int ang){
   int pulse = map(ang,0, 180, SERVOMIN,SERVOMAX); // mapear el ángulo de 0 a 180 a Servo min y Servo max
 // Serial.print("Ángulo: ");Serial.print(ang);
 // Serial.print(" pulso: ");Serial.println(pulso);
   return pulse;
}