#include <PID_v1_bc.h>

#define ledRojo1 A0
#define ledRojo2 A1
#define ledVerde1 A2
#define ledVerde2 A3
#define pote A6
#define TRIG_PIN 8
#define ECHO_PIN 9

// PID
double Entrada, Retro, SalidaControlador,rta,Test;

double Kp=2, Ki=5, Kd=1;
// Creamos una instancia del controlador PID (Definido únicamente para Pitch(Rotación "hacia los lados")
// o Roll(Rotación "hacia el frente y atrás"), la otra se hace de igual manera)
PID myPID(&Retro, &rta, &Entrada, Kp, Ki, Kd, DIRECT);

// Resto de variables


void setup()
{
  // Inicializamos las variables a las que estamos conectados
  Retro = 0;
  Entrada = 0;
  rta = 0;
  Serial.begin(115200);
  pinMode(ledRojo1, OUTPUT);
  pinMode(ledRojo2, OUTPUT);
  pinMode(ledVerde1, OUTPUT);
  pinMode(ledVerde2, OUTPUT);
  pinMode(pote, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Encendemos el PID
  myPID.SetMode(AUTOMATIC);
}
void loop()
{
  // Leemos el valor de entrada
  // Ping

  Retro = MPUOutput();      //Retro
  Serial.print("\n-----------------------------------");
  Serial.print("\nDistancia medida: ");
  Serial.println(Retro);
  
  Entrada = userOutput();  //Entrada
  Serial.print("Petición del Usuario: ");
  Serial.print(Entrada);

  // Calculamos el valor de salida con el PID
  myPID.Compute();
  // Escribimos el valor de salida
  Serial.print("\nSalida del controlador: ");
  Serial.print(SalidaControlador);
  droneControl(SalidaControlador);

  //rta
  Serial.print("\nRespuesta: ");
  Serial.print(rta);

  delay(1000);

}
// (Elemento de Medición) Lee por puerto serie los bits que envía el MPU6050 y los convierte en una inclinación en grados
// Utilizamos un Ping para su representación, directamente varía el ángulo con respecto a la distancia ya que
// es solidaria con la aceleración y la velocidad angular.

double MPUOutput()
{
  long duration, distanceCm;

  digitalWrite(TRIG_PIN, LOW);  //para generar un pulso limpio ponemos a LOW 4us
  delayMicroseconds(4);
  digitalWrite(TRIG_PIN, HIGH);  //generamos Trigger (disparo) de 10us
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  duration = pulseIn(ECHO_PIN, HIGH);  //medimos el tiempo entre pulsos, en microsegundos

  distanceCm = duration * 0.034 / 2;   //convertimos a distancia, en cm
  return distanceCm;
}

// (Entrada) Lee los pulsos enviados por radiofrecuencia por el usuario, los interpreta según el tiempo en ON y OFF y los transforma en una inclinación en grados
// Utilizamos un pote para su representación, como solo estamos simulando una de las rotaciones, requerimos uno
// solo
double userOutput()
{
  double valor = analogRead(pote);
  valor = map(valor,0,1023,0,255);
  return valor;
}

// (Salida a los actuadores) Según la entrada recibida y su retroalimentación, modifica la velocidad de los motores según corresponda
// para su estabilización (tiene en cuenta el PID del Pitch y del Roll)
// Utilizamos 4 leds para la representación de los motores, para no estudiar cómo se controla un dron en su
// totalidad, representamos el giro como 2 aumentando su luminosidad y los otros 2 disminuyendola.
void droneControl(double SalidaControlador)
{
  double rojo;
  double verde;
  if(SalidaControlador>255/2)
  {
    rojo = 255/2 + SalidaControlador/2;
    verde = 255/2 - SalidaControlador/2;
  }
  else
  {
    verde = 255/2 + SalidaControlador/2;
    rojo = 255/2 - SalidaControlador/2;
  }
  analogWrite(ledRojo1,rojo);
  analogWrite(ledRojo2,rojo);
  analogWrite(ledVerde1,verde);
  analogWrite(ledVerde2,verde);
}