/* Las librerias estan en: 
https://codebender.cc/library/PID_v1#PID_v1.h

en PID_v1.cpp se cambia de #include <PID_v1.h> a #include "PID_v1.h"
*/

#include <ESP32Servo.h>

#include "PID_v1.h"

Servo myservo;
double Setpoint;
double Input;
double Output;

// Potentiometer1 is connected to GPIO 34 (Analog ADC1_CH6) 
const int potPin1 = 34;
//Potentiometer2 is connected to GPIO 36 (Analog ADC1_CH0)
const int potPin2 = 36;

//NOTE: ADC INPUT CHANNELS ONLY WORKS WITH INTEGRER VALUES
// variable for storing the potentiometer1 value
int potValue1 = 0;
// variable for storing the potentiometer1 value
int potValue2 = 0;

double Kp=1.0, Ki=0.0005, Kd=0.05;   //1, 0.0015, 0.0005: OK.1, 3, 0.01:OK 2, 5, 1
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup()
{
  Serial.begin(115200);
  myservo.attach(12); 
  pinMode(potPin1, INPUT);
  pinMode(potPin2, INPUT);
  //Setpoint = 90;   //Valor deseado.
  //myPID.SetMode(AUTOMATIC);
}

void loop()
{
  myPID.SetMode(AUTOMATIC);
  potValue1 = analogRead(potPin1);
  Input=(potValue1*180.0)/4095;
  potValue2 = analogRead(potPin2);
  Setpoint = (potValue2*180.0)/4095;
  myPID.Compute();
  myservo.write(Output);
  delay(100);
  Serial.print("Setpoint: ");
  Serial.print(Setpoint);
  Serial.print("  Val entrada: ");
  Serial.print(Input);
  Serial.print("  Val salida: ");
  Serial.println(Output);
}