//Ejemplo de un controlador PID creado con freeRTOS
//Demuestra el uso de Colas (Queues) para enviar 
//mensajes entre tareas y zonas de exclusión mutua (Mutex)
//usando semáforos para evitar efectos indeseables en el display
//cuando dos tareas acceden simulatáneamente a este recurso compartido.
//
//Tarea #1: Implementa el PID aplicando la ley de control cada Ts=1,3s
//        Usa cola para enviar medida de temperatura
//        Usa setpoint almacenado
//Tarea #2: Visualiza medida
//          Usa cola para leer medida temperatura
//          Escribe en display la medida 
//Tarea #3: Cambio y Visualización del setpoint
//          Lee botones y modifica setpoint
//          Escribe en display el nuevo setpoint
// Jorge Iván Marín Hurtado
// Universidad del Quindío
// [email protected]


#include <LiquidCrystal_I2C.h>

#define ADC_SENS  35
#define UP_SETPOINT 12
#define DOWN_SETPOINT 14

int r = 512;  //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;

//Semáforo
SemaphoreHandle_t xSemaphore = NULL;

//Referencia a la tarea para modificar el setpoint
TaskHandle_t TaskHandleSetPoint;

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

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

 xTaskCreate(
  TaskSetPoint, "Setpoint",
  2048, NULL, //Stack
  1, //Prioridad 1 (Baja)
  &TaskHandleSetPoint
 );

 //Suspende la tarea de lectura del SetPoint para que se ejecute solo
 //después de haber inicializado el display en la tarea Display
 vTaskSuspend(TaskHandleSetPoint);

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

 //Crea semáforo
 xSemaphore = xSemaphoreCreateMutex();
}

void loop(){
  delay(10);
}


void TaskController(void *pvParameters) {
 TickType_t xLastWakeTime;
 while(1) {
  xLastWakeTime = xTaskGetTickCount();
  //Lee la señal de salida de la planta
  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
    }
  }

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

void TaskDisplay(void *pvParameters) {
 int measure;

  lcd_1.init();
  lcd_1.backlight();
  lcd_1.setCursor(0, 0);
  lcd_1.print("Temp: ");
  lcd_1.setCursor(0, 1);
  lcd_1.print("SetPoint: ");
      
 //Permite que la tarea de lectura del SetPoint inicie ya que el 
 //display ha sido inicializado
 vTaskResume(TaskHandleSetPoint);

 while(1) {
  //Muestra la temperatura actual
  if( xQueueReceive( xQueueSens, &measure, 10 )==pdTRUE ) {
    if (xSemaphore!=NULL) {
      if( xSemaphoreTake( xSemaphore, (TickType_t) 10 ) == pdTRUE ) {
        lcd_1.setCursor(6, 0);
        lcd_1.print(measure/10.24); lcd_1.print("   ");
        //Libera el recurso compartido
        xSemaphoreGive( xSemaphore );
      } 
    }
  }
  vTaskDelay( pdMS_TO_TICKS(1000) );
 }
}


void TaskSetPoint(void *pvParameters) {
 int setpoint;

 pinMode(UP_SETPOINT, INPUT_PULLUP);
 pinMode(DOWN_SETPOINT, INPUT_PULLUP);

 while(1) {
  if (digitalRead(UP_SETPOINT)==LOW) {
    r+=10;
    if (r>1024) r=1024;
  }
  if (digitalRead(DOWN_SETPOINT)==LOW) {
    r-=10;
    if (r<10) r=0;
  }
  if (xSemaphore!=NULL) {
    if( xSemaphoreTake( xSemaphore, (TickType_t) 10 ) == pdTRUE ) {
      lcd_1.setCursor(10, 1);
      lcd_1.print(r/10.24); lcd_1.print("   ");
      //Libera el recurso compartido
      xSemaphoreGive( xSemaphore );
    } 
  }
  vTaskDelay( pdMS_TO_TICKS(100) );
 }
}