//Ejemplo de un controlador PID creado con freeRTOS
//Demuestra el uso de Colas (Queues) par intercambiar 
//mensajes entre ambas tareas
//Tarea #1: Implementa el controlador PID y 
//          envía los valores de lectura y setpoint a la Tarea 2
//Tarea #2: Visualiza la lectura actual y el set-point en el display
//          usando los valores enviados por la Tarea 1
// 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);

//Cola de mensajes
QueueHandle_t xQueueSens;
QueueHandle_t xQueueRef;

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

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

 //Crea colas de máximo 10 elementos
 xQueueSens = xQueueCreate( 10, sizeof(int) );
 xQueueRef  = xQueueCreate( 10, sizeof(int) );
}

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);

  //Envía datos a las colas
  if( xQueueSens != 0 ) {
    if (xQueueSend(xQueueSens, &y, 0) == pdTRUE) {
      // Mensaje enviado satisfactoriamente
    }
  }
  if( xQueueRef != 0 ) {
    if (xQueueSend(xQueueRef, &r, 0) == pdTRUE) {
      // Mensaje enviado satisfactoriamente
    }
  }

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

void TaskDisplay(void *pvParameters) {
 int measure;
 int setpoint;

 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
  if( xQueueReceive( xQueueSens, &measure, 10 )==pdTRUE ) {
    lcd_1.setCursor(6, 0);
    lcd_1.print(measure/10.24); lcd_1.print("   ");
  }
  //Muestra la referencia
  if( xQueueReceive( xQueueRef, &setpoint,10 )==pdTRUE ) {
    lcd_1.setCursor(10, 1);
    lcd_1.print(setpoint/10.24); lcd_1.print("   ");
  }
  vTaskDelay( pdMS_TO_TICKS(1000) );
 }
}