/*
     * Copyright (C) 2021 - Cuautli Garcia - All Rights Reserved
     *
     * FabAcademy
     *
     * Version 1.0
     * Autor: Cuautli Garcia - cuautli.garciaa@gmail.com
     *
     */
//Required libraries
#include <WiFi.h>
#include <Adafruit_PWMServoDriver.h>//Library for PCA9685
#include <Wire.h>             //Library for I2C communication

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);//Initialize PCA9685
#define SERVOMIN  100         //Minimum pulse length count (out of 4096)
#define SERVOMAX  500         //Maximum pulse length count (out of 4096)
#define SERVO_FREQ 50         //Servomotor frequency for analog servos at 50Hz
 
// Wifi credentials
const char* ssid = "HOME_2.4G";
const char* password =  "5GaRcIaS";
//Set the web server port number to 80
WiFiServer wifiServer(80);
//String to store the received information from the computer
String cadenarecibida;
int pos; //Variable to save the position of the servomotors

void setup() { 
  Serial.begin(115200);//Initialize the serial protocol
  delay(1000);        //Wait a second
  WiFi.begin(ssid, password);//Start the WiFi protocol
  //While the wifi is not connected
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);      //Wait a second
    //Display the following text
    Serial.println("Connecting to WiFi..");
  }
  //When the Wifi is connected
  Serial.println("Connected to the WiFi network");
  Serial.println(WiFi.localIP());//Prin the IP addres
  wifiServer.begin();//Start the Wifi server
  Serial.println("");         //Print a space
  pwm.begin();                //Initialize the PCA
  pwm.setOscillatorFrequency(27000000);//Set the clock oscillator at 27 MHz
  pwm.setPWMFreq(SERVO_FREQ); //Frecuecia PWM de 50Hz o T=20ms
  delay(100);                 //Delay of 100 ms
}
 
void loop() {
  //Start a Wifi client
  WiFiClient client = wifiServer.available();
  if (client) {//If a client is detected
    while (client.connected()) {//While the client is connected
      while (client.available()>0) {//While the bus is available
        char c = client.read(); //Read the information from the interface
        cadenarecibida += c;    //Save the information
        if (c == 'a'){          //If there is an a detected
          cadenarecibida.replace("a","");//Erase the a and save the whole string
          Serial.print("Servo 1: ");    //Print Servo 1
          Serial.print(cadenarecibida); //Print the value of the first servo
          pos = cadenarecibida.toInt(); //Convert the text into a number
          //Calculate the pulse length depending on the given value
          int real = (pos*(SERVOMAX-SERVOMIN)/180)+SERVOMIN;
          Serial.print(" -> ");         //Print an arrow
          Serial.println(real);         //Print the pulse length
          pwm.setPWM(0, 0, real);       //Send the right value to the servo 0
          cadenarecibida="";            //Clean the string
        }else if(c == 'b'){     //If there is a b detected
          cadenarecibida.replace("b","");//Erase the b and save the whole string
          Serial.print("Servo 2: ");    //Print Servo 2
          Serial.print(cadenarecibida); //Print the value of the first servo
          pos = cadenarecibida.toInt(); //Convert the text into a number
          //Calculate the pulse length depending on the given value
          int real = (pos*(SERVOMAX-SERVOMIN)/180)+SERVOMIN;
          Serial.print(" -> ");         //Print an arrow
          Serial.println(real);         //Print the pulse length
          pwm.setPWM(1, 0, real);       //Send the right value to the servo 1
          cadenarecibida="";            //Clean the string
        }        
      }
      delay(10);                        //Delay of 10 ms
    }
    client.stop();                      //Stop the client
    Serial.println("Client disconnected");
  }
}