/***********************************************************

  En esta práctica se pide programar el control PID de un motor CC en posición.
  La referencia se recibe a través del puerto serie.
  Está realizada en M5, pero se comentan las líneas relacionadas
  El control no lo realiza en grados, sino en pulsos

************************************************************/
//#include <M5Core2.h>
#include <Ticker.h>

Ticker blinker;

// Definición de puertos

#define ledChannel 0 // Canal PWM, puede ser de 0 a 15
#define PWM_PIN 13 // Pin al que se conecta el dispositivo
#define SENTIDO_PIN 14 // Pin al que se conecta el dispositivo
#define CHANNEL_A 27
#define CHANNEL_B 19

// Definición de variables
volatile int pulsos;
const int frequency = 1000; // Hz
const int resolution = 10; // Resolucion en bits (de 1 a 15) normalmente se utilizan 10
// ya que así el duty cycle es un numero en el rango 0-1023

// Constantes del PID
const float Kp = 1;
const float Ki = 0.01;
const float Kd = 1;

volatile int actuacion_sentido;
volatile int actuacion_pwm;
volatile int actuacion;

volatile int error_acumulado;
volatile int error_actual;
volatile int error_anterior;

volatile int referencia;

void ICACHE_RAM_ATTR InterruptChannel_A()
{
  if (digitalRead(CHANNEL_A) != digitalRead(CHANNEL_B))
  {
    pulsos++;
  }
  else
  {
    pulsos--;
  }
}

void ICACHE_RAM_ATTR InterruptChannel_B()
{
  if (digitalRead(CHANNEL_A) == digitalRead(CHANNEL_B))
  {
    pulsos++;
  }
  else
  {
    pulsos--;
  }
}

int PIDControl(int ref) {

  float salida; // variable de salida, se le hará un cast a int

  // Actualizo los errores
  error_anterior = error_actual;
  error_actual = pulsos - ref;
  error_acumulado += error_actual;

  // Realizo un reset del error acumulado en caso de que la salida tenga error nulo
  // No se explica en ningún lado, así que mejor no ponerlo, pero en clase práctica
  // me funcionó
  if (error_actual == 0) {
    error_acumulado = 0;
  }

  // Calculo el valor de la salida
  salida = Kp * error_actual + Ki * error_acumulado + Kd * (error_actual - error_anterior);


  // Saturo los límites de la salida, en este caso la salida será utilizada
  // como dutyCycle de un PWM de resolución de 10 bits (0-1023)
  if (salida > 1023) {
    salida = 1023;
  }
  else if (salida < -1023) {
    salida = -1023;
  }

  return (int)salida;
}


void Control(void) {

  // A partir de la referencia obtengo la actuación
  actuacion = PIDControl(referencia);

  // Determino el sentido de giro a partir del valor de la actuación
  if (actuacion > 0) {
    actuacion_sentido = 1;
  }
  else {
    actuacion_sentido = 0;
  }

  // El valor de la actuación PWM debe estar en el rango 0-1023,
  // por lo tanto, se utiliza el valor absoluto
  actuacion_pwm = abs(actuacion);

  // Genero las actuaciones
  digitalWrite(SENTIDO_PIN, actuacion_sentido);
  ledcWrite(ledChannel, actuacion_pwm);
}



void setup() {
  //M5.begin();
  Serial.begin(9600); // Inicio la comunicacion con el puerto serie
  Serial.println("Conectado!");


  // Selecciono los pines como entrada
  pinMode(CHANNEL_A, INPUT_PULLUP);
  pinMode(CHANNEL_B, INPUT_PULLUP);

  //Inicializo las variables
  pulsos = 0;
  error_acumulado = 0;
  error_actual = 0;
  error_anterior = 0;

  // Activo las interrupciones
  attachInterrupt(digitalPinToInterrupt(CHANNEL_A), InterruptChannel_A, CHANGE);
  attachInterrupt(digitalPinToInterrupt(CHANNEL_B), InterruptChannel_B, CHANGE);

  ledcSetup(ledChannel, frequency, resolution); // Configuración del PWM
  ledcAttachPin(PWM_PIN, ledChannel); // Asigno el PWM al pin
  pinMode(SENTIDO_PIN, OUTPUT);
  digitalWrite(SENTIDO_PIN, LOW);

  //Cada 10ms realizo el control
  blinker.attach(0.01, Control);
}

void loop() {

  int ref = Serial.parseInt();

  // Por defecto se cree el puerto serie que le mando un 0 si no escribo nada
  if (ref != 0) {
    referencia = ref;
    Serial.println(referencia);
  }

  Serial.println(pulsos);

}