//Ejemplo de un controlador PID creado con freeRTOS
//Tarea #1: Implementa el controlador PID
//Tarea #2: Visualiza la lectura actual y el set-point en el display
// Jorge Iván Marín Hurtado
// Universidad del Quindío
// [email protected]

#include <LiquidCrystal_I2C.h>

#define ADC_REF 34
#define ADC_SENS  35

int r;  //Señal de referencia
int y;  //Señal de salida de la planta
int u;	//Salida del controlador
int e, e1 = 0, u1 = 0;  //Variables internas del controlador PI
float Kp = 0.5; //Constante proporcional
float Ki = 0.3; //Constante integral
float Ts = 1.3; //Periodo de muestreo

LiquidCrystal_I2C lcd_1(0x27, 16, 2);

void setup() {
 xTaskCreate(
  TaskController, "Controller",
  1024, NULL, //Stack
  2, //Prioridad 2 (Alta)
  NULL
 );

 xTaskCreate(
  TaskDisplay, "Display",
  2048, NULL, //Stack
  1, //Prioridad 1 (Baja)
  NULL
 );
}

void loop(){
  delay(10);
}


void TaskController(void *pvParameters) {
 TickType_t xLastWakeTime;
 while(1) {
  xLastWakeTime = xTaskGetTickCount();
  //Lee la señal de referencia y salida de la planta
  r = analogRead(ADC_REF);
  y = analogRead(ADC_SENS);
  
  //Realiza la acción de control
  e = r-y;
  u = u1 + (Kp + Ki*Ts)*e + Kp*e1;
  u1 = u;
  e1 = e;
  
  //Envía la acción de control a la planta 
  dacWrite(DAC1, u);

  // Espera el tiempo de muestreo
  vTaskDelayUntil( &xLastWakeTime, pdMS_TO_TICKS(1300) );
 }
}

void TaskDisplay(void *pvParameters) {
 lcd_1.init();
 lcd_1.backlight();
 lcd_1.setCursor(0, 0);
 lcd_1.print("Temp: ");
 lcd_1.setCursor(0, 1);
 lcd_1.print("SetPoint: ");

 while(1) {
  //Muestra la temperatura actual
  lcd_1.setCursor(6, 0);
  lcd_1.print(y/10.24); lcd_1.print("   ");
  //Muestra la referencia
  lcd_1.setCursor(10, 1);
  lcd_1.print(r/10.24); lcd_1.print("   ");
  vTaskDelay( pdMS_TO_TICKS(1000) );
 }
}