 /******************************************************************************
  FreeRTOS - ARM7TDMI_LPC2114: 
         aplicacion al control de la plataforma robotica RoMAA
*******************************************************************************

Authors:
- Baudino, Martin Sebastian.
- Martini, Lucas.
- Palomeque, Nestor.
- Perez, Santiago.
- De la Cruz Thea, Jeremias.

Directors:
- Ing. Perez Paina, Gonzalo.
- Ing. Gaydou, David. 


Centro de Investigacion en Informatica para la Ingenieria (CIII)
Universidad Tecnologica Nacional - Facultad Regional Cordoba
Cordoba, Argentina.

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


/********************************* INCLUDES **********************************/
#include "main.h"


/******************************* GLOBAL VARIABLES ****************************/

/* Handles para semáforos de sicronización entre tareas */
xSemaphoreHandle xSemaphorePWMs=NULL, xSemaphoreVWRef=NULL,xSemaphoreMotorPIDParams=NULL, \
	xSemaphoreVWPIDParams=NULL, xSemaphoreLoopStatus=NULL, xSemaphoreEncCountTemp=NULL,   \
	xSemaphoreXYTheta=NULL, xSemaphoreWheelDist=NULL, xSemaphoreMotorLinearSpeed=NULL,    \
	xSemaphoreMotorAngularSpeed=NULL, xSemaphoreKinematic=NULL;

// Handle para suspend y resume de la tarea de odometría
xTaskHandle xLoggingHandle;
 
/******************************************* 
Uniones y Variables para Recepcion y Envio */
uchar_to_float_t uchar_to_float;
uchar_to_int_t uchar_to_int;

float send_float_data[MAX_FLOAT_DATA];
int   send_int_data[MAX_INT_DATA];

/* Estructuras */
/********** 
Config from Flash ***/
sInFlashConfiguration     ConfigParams;
sInFlashConfiguration * p_ConfigParams;
/********** 
Motores ***/
motor_loop_t loop_motor_l;
motor_loop_t loop_motor_r;
pid_t vw_pid;
pid_t v_pid;

/*********** 
Encoders ***/
encoder_t encoder_l;
encoder_t encoder_r;


/* Variables generales */
unsigned char loop_status;
int 	      pwm_motor_izq, pwm_motor_der;
float 	      w_ref, w_out;
float         v_ref, v_l_out, v_r_out;

/* Variables de odometria */
float wheel_distance_left, wheel_distance_right, \
      center_distance, center_phi, \
      x,y,theta;

/* Variables y banderas de logging */
unsigned char log_flags, logging_send_flag;

/* Buffer para envío de datos */
float 	log_float_data[MAX_FLOAT_DATA];
int 	log_int_data[MAX_INT_DATA];

/* Modo de comandos de comunicación */
char ContinuousMode;

/* Variables para el ADC */
float VBattery;
int adcValue;

/* In Application Programming (IAP) */
// Buffers utilizados
unsigned int commandIAP[5];
unsigned int resultIAP[3];
// Función
typedef void (*IAP)(unsigned int [],unsigned int[]);
IAP iap_entry;





/*********************************** MAIN ************************************/

/**
 * \fn int main( void )
 * \brief Inicializa todas las otras tareas, y luego inicia el scheduler.
 * \return En funcionamiento normal, el programa nunca llega al retorno en
 *         main, por lo tanto, por mas que main() este declarada como "int",
 *         nunca devuelve ningun valor.
 * \param  configMINIMAL_STACK_SIZE * 8 = 1024Bytes
 */
int main( void ) {
    
  /* Inicializa el hardware */
  prvSetupHardware();
  /* Inicializa todas las variables, estructuras y configuraciones */
  init_all();


  /* Creacion de las tareas de la aplicacion */
  xTaskCreate( TaskPIDLoop, 		( signed portCHAR * ) "PIDLoop", \
               configMINIMAL_STACK_SIZE	* 1, NULL, mainTaskPIDLoop_PRIORITY, NULL);
  xTaskCreate( TaskSerialCommunication, ( signed portCHAR * ) "Communication", \
               configMINIMAL_STACK_SIZE * 1, NULL, mainTaskSerialCommunication_PRIORITY, NULL );       
  xTaskCreate( TaskOdometry, 		( signed portCHAR * ) "Odometry", \
               configMINIMAL_STACK_SIZE * 4, NULL, mainTaskOdometry_PRIORITY, NULL);
  xTaskCreate( TaskLogging, 		( signed portCHAR * ) "Logging", \
               configMINIMAL_STACK_SIZE * 1, NULL, mainTaskLogging_PRIORITY, &xLoggingHandle);
  xTaskCreate( TaskRTOSAlive, 		( signed portCHAR * ) "ALive", \
               configMINIMAL_STACK_SIZE * 1, NULL,mainTaskRTOSALive_PRIORITY, NULL);

  // Suspendo la tarea de logging para que no se ejecute
  vTaskSuspend(xLoggingHandle);

  // Inicio el scheduler
  vTaskStartScheduler();

  return 0; // El scheduler es un loop infinito, por lo tanto el programa nunca
            // deberia llegar aqui.
 }



/********************************* Tasks *************************************/
/* ---------------------------- TaskMODELO --------------------------------- */
/*  Es un modelo para crear una tarea nueva. */

/*
   static void TaskMODELO( void *pvParameters )
   {
     // Para bloquear la tarea por 1000ms, por ejemplo. El numero es 
     // exactamente el tiempo deseado en ms para bloquear la tarea.
     const portTickType xDelay = 1000 / portTICK_RATE_MS;

     // Cuando los parametros no son usados en la tarea. 
     ( void ) pvParameters;

     for(;;){
       
       // Aca va el codigo de la tarea.
       
       // Devuelve el control al Scheduler y bloquea la tarea por el tiempo 
       // xDelay.
       vTaskDelay( xDelay );
   }        
}

*/



/* --------------------------- TaskRTOSAlive ------------------------------- */
/* Es una tarea que tooglea un LED (por defecto el LED ubicado en la placa, 
   P0.8), para tener un simple del correcto funcionamiento del FreeRTOS. */

static void TaskRTOSAlive( void *pvParameters ) {
  portTickType xDelay = ConfigParams.rtos_alive_ms / portTICK_RATE_MS;
  int accumRTOSAlive=0;
  int accum1Level=0;
  int accum2Level=0;
  int accumAverage=0;
  float VBatteryAccum=0.0;
  ( void ) pvParameters;

  // Primera medición forzada para evitar esperar ADC_COUNTER_SAMPLING 
  adcValue=adc_conversion_bloq(AN1_P028);
  VBattery= ((adcValue*ADC_BATTERY_MAX_VOLTAGE)/ConfigParams.ADCConvConstant);   
  
  for(;;)
  {
    xDelay = ConfigParams.rtos_alive_ms / portTICK_RATE_MS;

    // Acumuladores para variar las frecuencias de toogle de leds
    accumRTOSAlive++;
    accum1Level++;
    accum2Level++;   

    // Toogle led RTOS
    if(accumRTOSAlive >= 1){
      gpio_toggle(GPIO_PIN0_5); // Led Hongo
      gpio_toggle(GPIO_PIN0_8); // Led PCB 
      accumRTOSAlive=0;
    }

    // Cada ADC_COUNTER_SAMPLING repeticiones, tomo una nueva muestra del ADC
    if(accum2Level >= ADC_COUNTER_SAMPLING){
      accum2Level=0; 
      accumAverage++; 
      //gpio_toggle(GPIO_PIN0_8); // Led PCB 
      adcValue=adc_conversion_bloq(AN1_P028);
      VBatteryAccum= VBatteryAccum + ((adcValue*ADC_BATTERY_MAX_VOLTAGE)\
                     /ConfigParams.ADCConvConstant);   
    }     

    /* Cada ADC_AVERAGE_SAMPLES muestras tomadas del ADC 
       (ADC_COUNTER_SAMPLING*ConfigParams.rtos_alive_ms*ADC_AVERAGE_SAMPLES)
       calculo el promedio */
    if(accumAverage == ADC_AVERAGE_SAMPLES){
      accumAverage=0;
      // Saco el promedio de las AVERAGE_ADC_SAMPLES muestras
      VBattery = VBatteryAccum/ADC_AVERAGE_SAMPLES;
      VBatteryAccum=0;
    }
    // Chequeo primer umbral de baterías
    if((VBattery <= ConfigParams.VBatteryHigh) && (VBattery > ConfigParams.VBatteryLow) && (accum1Level>=5)){	
      gpio_toggle(GPIO_PIN0_6);
      accum1Level=0;
    }
    else{
      // Chequeo segundo umbral de baterías
      if(VBattery <= ConfigParams.VBatteryLow){    
        gpio_toggle(GPIO_PIN0_6);
        if(ConfigParams.checkBattery==TRUE){
          // Deshabilito las llaves H
          gpio_set(MOTOR1_ENABLE_PIN, GPIO_ON);
          gpio_set(MOTOR2_ENABLE_PIN, GPIO_ON);
        }
      }
      else{
        gpio_set(GPIO_PIN0_6, GPIO_OFF);
      }
    }
    
    vTaskDelay( xDelay );

    }        
}


/* ---------------------------- TaskPIDLoop -------------------------------- */
/* Esta tarea se encarga de implementar el tipo de control elegido: Lazo 
   Abierto (OPEN_LOOP), Lazo cerrado completamente (CLOSE_LOOP) o lazo cerrado 
   de motor unicamente (MOTOR_LOOP).
   
   Primero realiza una comprobacion de la velocidad cero del RoMAA-II y luego 
   calcula los PWM de cada caso segun el lazo y las constantes PID establecidas
   anteriormente. */

static void TaskPIDLoop( void *pvParameters ) {
  
  /*const*/ portTickType xDelay = ConfigParams.loop_pid_ms / portTICK_RATE_MS;
  ( void ) pvParameters;


  for(;;)
  {
    xDelay = ConfigParams.loop_pid_ms / portTICK_RATE_MS;
    gpio_toggle(GPIO_PIN0_9);


    /********************************************** 
    / Chequeo de velocidad cero de los dos motores 
    ***********************************************/
    // Deshabilito interrupciones
    portENTER_CRITICAL();
      // Chequea velocidad cero
      if( encoder_l.recent_pulse >= (T_ZERO_SPEED/ConfigParams.odometry_ms) ){ // Se incremente en OdometryTask
        encoder_l.period_pclk = 0x7FFFFFFF;    	// Cuando el romaa esta quieto.
        encoder_l.edge_previous = 0;
      }
      if( encoder_r.recent_pulse >= (T_ZERO_SPEED/ConfigParams.odometry_ms) ){ // Se incremente en OdometryTask
        encoder_r.period_pclk = 0x7FFFFFFF;    // Cuando el romaa esta quieto.
        encoder_r.edge_previous = 0;
      }
      // Se copian los valores en variables temporales
      encoder_l.period_pclk_temp = encoder_l.period_pclk; 
      encoder_r.period_pclk_temp = encoder_r.period_pclk;
    // Habilito interrupciones
    portEXIT_CRITICAL();


    /***********************************************************
    / Seteo de velocidad angular y linear, caso que sean cero 
    / o distintas de cero
    ************************************************************/
    // Espera los semáforos para operar
    if(( xSemaphoreTake( xSemaphoreMotorAngularSpeed, ( portTickType ) portMAX_DELAY ) == pdTRUE )&& \
       ( xSemaphoreTake( xSemaphoreKinematic,         ( portTickType ) portMAX_DELAY ) == pdTRUE )&& \
       ( xSemaphoreTake( xSemaphoreMotorLinearSpeed,  ( portTickType ) portMAX_DELAY ) == pdTRUE )){
      // Motor izquierdo con velocidad cero
        if (encoder_l.period_pclk_temp == 0x7FFFFFFF){
          loop_motor_l.angular_speed = 0;
          loop_motor_l.linear_speed = 0;
      }
      // Motor derecho con velocidad distinta de cero
      else {
        loop_motor_l.angular_speed = \
                                 (float) N_PCLK_TO_W / encoder_l.period_pclk_temp;
        loop_motor_l.linear_speed = \
                           loop_motor_l.feedback_cte * loop_motor_l.angular_speed;
      }

      // Motor derecho con velocidad cero
      if (encoder_r.period_pclk_temp == 0x7FFFFFFF){
        loop_motor_r.angular_speed = 0;
        loop_motor_r.linear_speed = 0;
      }
      // Motor derecho con velocidad distinta de cero
      else{
        loop_motor_r.angular_speed = \
                                 (float) N_PCLK_TO_W / encoder_r.period_pclk_temp;
        loop_motor_r.linear_speed = \
                           loop_motor_r.feedback_cte * loop_motor_r.angular_speed;
      }

      // Devuelve los semáforos
      xSemaphoreGive(xSemaphoreMotorAngularSpeed);
      xSemaphoreGive(xSemaphoreMotorLinearSpeed);
      xSemaphoreGive(xSemaphoreKinematic);
    }

    /*************************************************************** 
    /
    / Chequeo del tipo de lazo, actualización PID y PWMs    
    /
    ****************************************************************/
    // Tomo el semáforo de modo de lazo
    if((xSemaphoreTake( xSemaphoreLoopStatus, ( portTickType ) portMAX_DELAY ) == pdTRUE )){	
      switch(loop_status) {
	/************************* 
	/ Modo Lazo Abierto
	**************************/ 
        case OPEN_LOOP:
   	  // Libera el semáforo de modo de lazo
          xSemaphoreGive( xSemaphoreLoopStatus );	
	  // Tomo semáforo o espero hasta que pueda tomarlo
          if((xSemaphoreTake( xSemaphorePWMs, ( portTickType ) portMAX_DELAY ) == pdTRUE )){	
	  // Podría poner un tiempo, para que no bloquee el lazo 

            if( (pwm_motor_izq >= (-SATURATION_PWM)) && \
                (pwm_motor_izq <= SATURATION_PWM)){
              pwm_set_value(PWM_MATCH5, (loop_motor_l.zero_offset  - pwm_motor_izq));      
            }
	    if( (pwm_motor_der >= (-SATURATION_PWM)) && \
                (pwm_motor_der <= SATURATION_PWM) ) {
              pwm_set_value(PWM_MATCH2, (loop_motor_r.zero_offset  - pwm_motor_der));
            }

            // Libero semáforo
            xSemaphoreGive( xSemaphorePWMs );
	  }

          break;
      

	/************************* 
	/ Modo Lazo Cerrado Total
	**************************/ 
        case CLOSE_LOOP:
  	  // Libera el semáforo de modo de lazo
          xSemaphoreGive( xSemaphoreLoopStatus );		
	  // Tomo semáforo o espero hasta que pueda tomarlo
          if(	(xSemaphoreTake( xSemaphoreVWRef, 	   ( portTickType ) portMAX_DELAY ) == pdTRUE ) && \
		(xSemaphoreTake( xSemaphoreVWPIDParams,    ( portTickType ) portMAX_DELAY ) == pdTRUE ) && \
		(xSemaphoreTake( xSemaphoreMotorPIDParams, ( portTickType ) portMAX_DELAY ) == pdTRUE ) && \
		(xSemaphoreTake( xSemaphoreKinematic,      ( portTickType ) portMAX_DELAY ) == pdTRUE ) && \
		(xSemaphoreTake( xSemaphorePWMs, 	   ( portTickType ) portMAX_DELAY ) == pdTRUE )){	

            //-------------------------------  w  -------------------------------//
            vw_pid.in = w_ref - (loop_motor_r.linear_speed - \
                        loop_motor_l.linear_speed) / ConfigParams.wheelbase;
            pid_update( &(vw_pid), AW_I_DISABLE, AW_O_DISABLE);
            w_out = (float)ConfigParams.wheelbase * vw_pid.out;

            //-------------------------------- v  -------------------------------//
            v_pid.in  = v_ref - (loop_motor_r.linear_speed + \
                        loop_motor_l.linear_speed) / 2;
            pid_update( &(v_pid),  AW_I_DISABLE, AW_O_DISABLE );
            v_l_out = v_pid.out - w_out;
            v_r_out = v_pid.out + w_out;


            //Comprueba saturacion del PWM del motor izquierdo
            if(v_l_out < (-SATURATION_PWM))
              v_l_out = -SATURATION_PWM;
            else if(v_l_out > SATURATION_PWM)
              v_l_out = SATURATION_PWM;

            //Comprueba saturacion del PWM del motor derecho
            if(v_r_out < (-SATURATION_PWM))
              v_r_out = -SATURATION_PWM;
            else if(v_r_out > SATURATION_PWM)
              v_r_out = SATURATION_PWM;


            pwm_set_value(PWM_MATCH2, (loop_motor_r.zero_offset  - (int)v_r_out));
            pwm_set_value(PWM_MATCH5, (loop_motor_l.zero_offset  - (int)v_l_out));	

            // Libero los semáforos
            xSemaphoreGive( xSemaphoreVWRef );
            xSemaphoreGive( xSemaphoreVWPIDParams );
            xSemaphoreGive( xSemaphoreMotorPIDParams );
            xSemaphoreGive( xSemaphoreKinematic );
            xSemaphoreGive( xSemaphorePWMs );
	 }
         break;
     
        default:
	  // Libera el semáforo en caso de que el modo seteado no exista (por prevención)
          xSemaphoreGive( xSemaphoreLoopStatus );		
          break;
      }
    }

    vTaskDelay( xDelay );        
  }
}




/* ----------------------------- TaskOdometry ------------------------------ */
/* En esta tarea se realizan los calculos de odometria necesarios. */
 
static void TaskOdometry( void *pvParameters )
{
  /*const*/ portTickType xDelay = ConfigParams.odometry_ms / portTICK_RATE_MS;
  ( void ) pvParameters;


  for(;;)
  {
    xDelay = ConfigParams.odometry_ms / portTICK_RATE_MS;
    gpio_toggle(GPIO_PIN0_10);

    if( xSemaphoreTake( xSemaphoreEncCountTemp, ( portTickType ) portMAX_DELAY ) == pdTRUE ){
      // Deshabilito interrupciones
      portENTER_CRITICAL();
        // Para determinar veocidad cero en PIDTask 
        encoder_l.recent_pulse++;
        encoder_l.recent_pulse++;
        // Para trabajar con temporales
        encoder_l.count_temp = encoder_l.count;
        encoder_r.count_temp = encoder_r.count;
      // Habilito interrupciones
      portEXIT_CRITICAL();

      //Calcula la diferencia de cuentas y guarda la cuenta actual para
      // el próximo ciclo de cálculo. Trabajo con temporales
      // Motor izquierdo
      encoder_l.delta_count = encoder_l.count_temp - encoder_l.count_previous;
      encoder_l.count_previous = encoder_l.count_temp;
      // Motor derecho
      encoder_r.delta_count = encoder_r.count_temp - encoder_r.count_previous;
      encoder_r.count_previous = encoder_r.count_temp;

      xSemaphoreGive (xSemaphoreEncCountTemp);
    }

    // Calcula wheel_distance, center_distance y center_phi
    if(( xSemaphoreTake( xSemaphoreWheelDist, ( portTickType ) portMAX_DELAY ) == pdTRUE ) && \
       ( xSemaphoreTake( xSemaphoreKinematic, ( portTickType ) portMAX_DELAY ) == pdTRUE )){

      wheel_distance_left = encoder_l.delta_count * (float)WHEEL_DIST_RESOLUTION;
      wheel_distance_right = encoder_r.delta_count * (float)WHEEL_DIST_RESOLUTION;
      center_distance = (wheel_distance_left + wheel_distance_right) / 2;
      center_phi = (wheel_distance_right - wheel_distance_left) / (float)ConfigParams.wheelbase;

      xSemaphoreGive (xSemaphoreWheelDist);
      xSemaphoreGive (xSemaphoreKinematic);
    }

    // Calcula x, y y theta
    if( xSemaphoreTake( xSemaphoreXYTheta, ( portTickType ) portMAX_DELAY ) == pdTRUE ){ 

      theta = theta + center_phi;   
      x = x + center_distance * cosf(theta + center_phi/2);
      y = y + center_distance * sinf(theta + center_phi/2);

      xSemaphoreGive (xSemaphoreXYTheta);
    }
 
    vTaskDelay( xDelay );
  }        
}




/* ----------------------------- TaskLogging ------------------------------- */
/*  Esta tarea carga en las variables correspondientes, datos de logging segun
    sea el tipo de datos solicitado a traves de los comandos de logging en la 
    tarea de comunicacion, para luego ser enviados por la tarea de comunicacion
    en su proxima ejecucion.
    LOG_VW_FLAG se calcula directamente en la tarea de comunicacion antes de 
    ser enviado. */

static void TaskLogging( void *pvParameters )
{
  /*const */ portTickType xDelay = ConfigParams.logging_ms / portTICK_RATE_MS;
  ( void ) pvParameters;


  for(;;)
  {
    xDelay = ConfigParams.logging_ms / portTICK_RATE_MS;
    //gpio_toggle(GPIO_PIN0_11);

    if (log_flags != LOG_STOP_FLAG)
    {
      switch(log_flags)
      {
        case LOG_WHEEL_V_FLAG:
          if( xSemaphoreTake( xSemaphoreMotorLinearSpeed, ( portTickType )  portMAX_DELAY ) == pdTRUE ){
            log_float_data[0] = loop_motor_l.linear_speed;
            log_float_data[1] = loop_motor_r.linear_speed;
            xSemaphoreGive(xSemaphoreMotorLinearSpeed);
	  }
          break;
        case LOG_WHEEL_W_FLAG:
          if( xSemaphoreTake( xSemaphoreMotorAngularSpeed, ( portTickType ) portMAX_DELAY ) == pdTRUE ){
            log_float_data[0] = loop_motor_l.angular_speed;
            log_float_data[1] = loop_motor_r.angular_speed;
            xSemaphoreGive(xSemaphoreMotorAngularSpeed);
	  }
          break;
        case LOG_WHEEL_D_FLAG:
          if( xSemaphoreTake( xSemaphoreWheelDist, ( portTickType ) portMAX_DELAY ) == pdTRUE ){
            log_float_data[0] = wheel_distance_left;
            log_float_data[1] = wheel_distance_right;
            xSemaphoreGive(xSemaphoreWheelDist);
	  }
          break;
        case LOG_ENC_COUNTER_FLAG:
    	  if( xSemaphoreTake( xSemaphoreEncCountTemp, ( portTickType ) portMAX_DELAY ) == pdTRUE ){
            log_int_data[0] = encoder_l.count_temp;
            log_int_data[1] = encoder_r.count_temp;
	    xSemaphoreGive(xSemaphoreEncCountTemp);
	  }
          break;
        case LOG_PWM_FLAG:
          if((xSemaphoreTake( xSemaphorePWMs, ( portTickType ) portMAX_DELAY ) == pdTRUE )){	
            // PWM del Motor izquierdo
            log_int_data[0]= loop_motor_l.zero_offset - pwm_get_value(PWM_MATCH5);    
            // PWM del Motor derecho 
            log_int_data[1]= loop_motor_r.zero_offset - pwm_get_value(PWM_MATCH2);   
	    xSemaphoreGive(xSemaphorePWMs); 
	  }
          break;
        case LOG_VW_FLAG:
          if(( xSemaphoreTake( xSemaphoreMotorLinearSpeed, ( portTickType ) portMAX_DELAY ) == pdTRUE )&&\
             ( xSemaphoreTake( xSemaphoreKinematic,        ( portTickType ) portMAX_DELAY ) == pdTRUE )){ 
            log_float_data[0] = \
                  ( loop_motor_l.linear_speed + loop_motor_r.linear_speed ) / 2;
            log_float_data[1] = \
                   ( loop_motor_r.linear_speed - loop_motor_l.linear_speed ) / \
                   (float)ConfigParams.wheelbase;
            xSemaphoreGive (xSemaphoreMotorLinearSpeed);
            xSemaphoreGive (xSemaphoreKinematic);
	  }    
          break;
        case LOG_ODOM_FLAG:
          if( xSemaphoreTake( xSemaphoreXYTheta, ( portTickType ) portMAX_DELAY ) == pdTRUE ){  
 
            log_float_data[0] = x;
            log_float_data[1] = y;
            log_float_data[2] = theta;

            xSemaphoreGive(xSemaphoreXYTheta);
	  }
          break;
        default:
	  break;
      }
      logging_send_flag = TRUE;
    }
    else{
      logging_send_flag = FALSE; // Para asegurar
    }


    vTaskDelay( xDelay );
  }        
}




/* ----------------------- TaskSerialCommunication ------------------------- */
/*  Esta tarea de comunicacion es la encargada de recibir los comandos 
    provenientes de la PC de a bordo y actuar correspondientemente. Toma los 
    paquetes del buffer circular de comunicacion serie de las CiiEmbLibs, y 
    carga en las variables globales correspondientes los datos recibidos, 
    cuando son comandos de seteo. Si son comandos de pedido de datos, arma el 
    paquete de datos a enviar y lo carga en el buffer circular. Para los 
    comandos de Logging, estos datos se cargan en la tarea de logging, pero se 
    envian desde esta tarea, para centralizar la comunicacion de datos.        */
 
static void TaskSerialCommunication( void *pvParameters )
{
  unsigned int count_char;
  char comando[MAX_COMMAND] = {0};
  float timeOutComm = 0;
  /*const*/ portTickType xDelay = ConfigParams.communication_ms / portTICK_RATE_MS;
  ( void ) pvParameters;


  for(;;)
  {

    // Se actualiza la frecuencia de repeticion de la tarea y se toogle uno de 
    // los LEDs on board.
    xDelay = ConfigParams.communication_ms / portTICK_RATE_MS;    

    // Lee el buffer de recepción
    count_char = receive_packet(comando,UART0);   
    if(count_char > 0)
    {
      // Reseteo timeout de comandos recibidos
      timeOutComm=0;

      gpio_toggle(GPIO_PIN0_11);

      switch(comando[0])
      {
	/************************* 
	/ Reseteo total del RoMAA
	**************************/
        case CMD_RESET:
          reset_all();
          break;

	/************************** 
	/ Comandos de comunicación
	***************************/
        case CMD_COMMANDS_CONTINUOUS_MODE:
          ContinuousMode = comando[1];
          break;
        case CMD_SET_REDUNDANCE:
          uchar_to_int.uc_data[0] = comando[1];
          uchar_to_int.uc_data[1] = comando[2];
          uchar_to_int.uc_data[2] = comando[3];
          uchar_to_int.uc_data[3] = comando[4];

          if(uchar_to_int.i_data==0) {
            com_set_redundance(0,UART0);
          } else {
            com_set_redundance(1,UART0);
            com_del_comm_errors(UART0);
          }
          break;
          
        case CMD_DEL_COMM_ERRORS:
          com_del_comm_errors(UART0);
          break;
          
        case CMD_GET_COMM_ERRORS:
          send_int_data[0] = com_get_comm_errors(UART0);
          send_packet((char*) send_int_data, 4,UART0);
          com_del_comm_errors(UART0);
          break;

	/*********************** 
	/ Comandos de velocidad
	************************/
        case CMD_SET_SPEED:
	  // Toma el semáforo de valores de referencia VW
          if( xSemaphoreTake( xSemaphoreVWRef, ( portTickType ) portMAX_DELAY ) == pdTRUE ){
	    // Toma el semáforo de modo de lazo
	    if(loop_status != CLOSE_LOOP){
	      // Toma el semáforo de modo de lazo
    	      if((xSemaphoreTake( xSemaphoreLoopStatus, ( portTickType ) portMAX_DELAY ) == pdTRUE )){	
    	        // Inicializacion de los PID del motor derecho, izquierdo y lazo vw
 	        pid_init (&v_pid,   v_pid.Kp,  ConfigParams.v_pid_ki,  v_pid.Kd);
 	        pid_init (&vw_pid,  vw_pid.Kp, vw_pid.Ki, vw_pid.Kd);

	        // Cierra el lazo
                loop_status = CLOSE_LOOP;
	        // Libera el semáforo de modo de lazo
                xSemaphoreGive( xSemaphoreLoopStatus );	
	      }
	    }

            uchar_to_float.uc_data[0] = comando[1];
            uchar_to_float.uc_data[1] = comando[2];
            uchar_to_float.uc_data[2] = comando[3];
            uchar_to_float.uc_data[3] = comando[4];
            v_ref = uchar_to_float.f_data;         

            uchar_to_float.uc_data[0] = comando[5];
            uchar_to_float.uc_data[1] = comando[6];
            uchar_to_float.uc_data[2] = comando[7];
            uchar_to_float.uc_data[3] = comando[8];
            w_ref = uchar_to_float.f_data;

            xSemaphoreGive( xSemaphoreVWRef );
	  }
          break;

        case CMD_GET_SPEED:
          if(( xSemaphoreTake( xSemaphoreMotorLinearSpeed, ( portTickType ) portMAX_DELAY ) == pdTRUE )&&\
             ( xSemaphoreTake( xSemaphoreKinematic,        ( portTickType ) portMAX_DELAY ) == pdTRUE )){
            send_float_data[0] = \
                   ( loop_motor_l.linear_speed + loop_motor_r.linear_speed ) / 2;
            send_float_data[1] = \
                   ( loop_motor_r.linear_speed - loop_motor_l.linear_speed ) / \
                   ConfigParams.wheelbase;
            send_packet((char*) send_float_data, 8,UART0);
            xSemaphoreGive (xSemaphoreMotorLinearSpeed);
            xSemaphoreGive (xSemaphoreKinematic);
	  }
          break;

	/***************************************************** 
	/ Comandos de seteo y pedido de tiempos de las tareas
	******************************************************/
        case CMD_SET_T_LOOP:
          ConfigParams.loop_pid_ms = comando[1];
          writeConfigToFlash();
          break;

        case CMD_GET_T_LOOP:
          send_packet(&ConfigParams.loop_pid_ms, 1,UART0);
          break;

        case CMD_SET_T_SAMPLING:     
          ConfigParams.logging_ms = comando[1];
          writeConfigToFlash();
          break;

        case CMD_GET_T_SAMPLING:
          send_packet(&ConfigParams.logging_ms, 1,UART0);
          break;

        case CMD_SET_T_ODOMETRY:
          ConfigParams.odometry_ms = comando[1];
          writeConfigToFlash();
          break;

        case CMD_GET_T_ODOMETRY:
          send_packet(&ConfigParams.odometry_ms, 1,UART0);
          break;

	/********************************************** 
	/ Comandos de seteo y pedido de parámetros PID
	***********************************************/
        case CMD_SET_V_PID:
          if( xSemaphoreTake( xSemaphoreMotorPIDParams, ( portTickType ) portMAX_DELAY ) == pdTRUE ){
            // Seteo Kp del motor 1 y 2
            uchar_to_float.uc_data[0] = comando[1];
            uchar_to_float.uc_data[1] = comando[2];
            uchar_to_float.uc_data[2] = comando[3];
            uchar_to_float.uc_data[3] = comando[4];
            ConfigParams.v_pid_kp = uchar_to_float.f_data;
            v_pid.Kp = ConfigParams.v_pid_kp;

            // Seteo Ki del motor 1 y 2
            uchar_to_float.uc_data[0] = comando[5];
            uchar_to_float.uc_data[1] = comando[6];
            uchar_to_float.uc_data[2] = comando[7];
            uchar_to_float.uc_data[3] = comando[8];
            ConfigParams.v_pid_ki = uchar_to_float.f_data;
            v_pid.Ki = ConfigParams.v_pid_ki;

            // Seteo Kd del motor 1 y 2
            uchar_to_float.uc_data[0] = comando[9];
            uchar_to_float.uc_data[1] = comando[10];
            uchar_to_float.uc_data[2] = comando[11];
            uchar_to_float.uc_data[3] = comando[12];
            ConfigParams.v_pid_kd = uchar_to_float.f_data;
            v_pid.Kd = ConfigParams.v_pid_kd;

            writeConfigToFlash();

            pid_init (&v_pid,   v_pid.Kp,  v_pid.Ki,  v_pid.Kd);

	    xSemaphoreGive( xSemaphoreMotorPIDParams );
  	  }
          break;

        case CMD_GET_V_PID:
          // Envia los valores de las constantes del lazo (ambos motores iguales K)
          send_float_data[0] = ConfigParams.v_pid_kp;
          send_float_data[1] = ConfigParams.v_pid_ki;
          send_float_data[2] = ConfigParams.v_pid_kd;
          send_packet((char*)(send_float_data), 12,UART0);
          break;

        case CMD_SET_VW_PID:
          if( xSemaphoreTake( xSemaphoreVWPIDParams, ( portTickType ) portMAX_DELAY ) == pdTRUE ){
            // Seteo Kp de lazo vw
            uchar_to_float.uc_data[0] = comando[1];
            uchar_to_float.uc_data[1] = comando[2];
            uchar_to_float.uc_data[2] = comando[3];
            uchar_to_float.uc_data[3] = comando[4];
            ConfigParams.vw_pid_kp = uchar_to_float.f_data;
            vw_pid.Kp = ConfigParams.vw_pid_kp;

            uchar_to_float.uc_data[0] = comando[5];
            uchar_to_float.uc_data[1] = comando[6];
            uchar_to_float.uc_data[2] = comando[7];
            uchar_to_float.uc_data[3] = comando[8];
            ConfigParams.vw_pid_ki = uchar_to_float.f_data;
            vw_pid.Ki = ConfigParams.vw_pid_ki;

            uchar_to_float.uc_data[0] = comando[9];
            uchar_to_float.uc_data[1] = comando[10];
            uchar_to_float.uc_data[2] = comando[11];
            uchar_to_float.uc_data[3] = comando[12];
            ConfigParams.vw_pid_kd = uchar_to_float.f_data;
            vw_pid.Kd = ConfigParams.vw_pid_kd;

            writeConfigToFlash();

    	    // Inicializacion de los PID del motor derecho, izquierdo y lazo vw
 	    pid_init (&vw_pid, vw_pid.Kp, vw_pid.Ki, vw_pid.Kd);

            xSemaphoreGive( xSemaphoreVWPIDParams );
	  }
          break;

        case CMD_GET_VW_PID:
          // Envia los valores de las constantes del lazo vw
          send_float_data[0] = ConfigParams.vw_pid_kp;
          send_float_data[1] = ConfigParams.vw_pid_ki;
          send_float_data[2] = ConfigParams.vw_pid_kd;
          send_packet((char*) send_float_data, 12,UART0);
          break;


	/***************************************** 
	/ Comandos de seteo y pedido de odometría
	******************************************/
        case CMD_SET_ODOMETRY:
          if( xSemaphoreTake( xSemaphoreXYTheta, ( portTickType ) portMAX_DELAY ) == pdTRUE ){    
	    if(loop_status != CLOSE_LOOP){
	      // Toma el semáforo de modo de lazo
    	      if((xSemaphoreTake( xSemaphoreLoopStatus, ( portTickType ) portMAX_DELAY ) == pdTRUE )){	
    	        // Inicializacion de los PID del motor derecho, izquierdo y lazo vw
 	        pid_init (&v_pid,   v_pid.Kp,  v_pid.Ki,  v_pid.Kd);
 	        pid_init (&vw_pid,  vw_pid.Kp, vw_pid.Ki, vw_pid.Kd);
	        // Cierra el lazo
                loop_status = CLOSE_LOOP;
	        // Libera el semáforo de modo de lazo
                xSemaphoreGive( xSemaphoreLoopStatus );	
	      }
	    }

            uchar_to_float.uc_data[0] = comando[1];
            uchar_to_float.uc_data[1] = comando[2];
            uchar_to_float.uc_data[2] = comando[3];
            uchar_to_float.uc_data[3] = comando[4];
            x = uchar_to_float.f_data;

            uchar_to_float.uc_data[0] = comando[5];
            uchar_to_float.uc_data[1] = comando[6];
            uchar_to_float.uc_data[2] = comando[7];
            uchar_to_float.uc_data[3] = comando[8];
            y = uchar_to_float.f_data;

            uchar_to_float.uc_data[0] = comando[9];
            uchar_to_float.uc_data[1] = comando[10];
            uchar_to_float.uc_data[2] = comando[11];
            uchar_to_float.uc_data[3] = comando[12];
            theta = uchar_to_float.f_data;
  
	    xSemaphoreGive(xSemaphoreXYTheta);
	  }
          break;

        case CMD_GET_ODOMETRY:
          if( xSemaphoreTake( xSemaphoreXYTheta, ( portTickType ) portMAX_DELAY ) == pdTRUE ){    
            send_float_data[0] = x;
            send_float_data[1] = y;
            send_float_data[2] = theta;
            send_packet((char*) send_float_data, 12,UART0);

	    xSemaphoreGive(xSemaphoreXYTheta);
	  }
          break;

        case CMD_RESET_ODOMETRY:
          if( xSemaphoreTake( xSemaphoreXYTheta, ( portTickType ) portMAX_DELAY ) == pdTRUE ){    
            x = 0;
            y = 0;
            theta = 0;
	    xSemaphoreGive(xSemaphoreXYTheta);
	  }
          break;

	/*********************************** 
	/ Comandos de seteo y pedido de PWM
	************************************/
        case CMD_SET_PWM:
          
        if( xSemaphoreTake( xSemaphorePWMs, ( portTickType ) portMAX_DELAY ) == pdTRUE ){// Podría poner un tiempo, para que no bloquee la comunicación y
											 // descarte el paquete.
	  if(loop_status != OPEN_LOOP){
    	    // Toma el semáforo de modo de lazo
    	    if((xSemaphoreTake( xSemaphoreLoopStatus, ( portTickType ) portMAX_DELAY ) == pdTRUE )){
	      // Abre el lazo 	 
 	      loop_status = OPEN_LOOP;    
	      // Libera el semáforo de modo de lazo
              xSemaphoreGive( xSemaphoreLoopStatus ); 
	    }
	  }

          // Seteo valor de PWM1 entre -1500 y 1500
          uchar_to_int.uc_data[0] = comando[1];
          uchar_to_int.uc_data[1] = comando[2];
          uchar_to_int.uc_data[2] = comando[3];
          uchar_to_int.uc_data[3] = comando[4];
          pwm_motor_izq = uchar_to_int.i_data;
	  
          // Seteo valor de PWM2 entre -1500 y 1500
          uchar_to_int.uc_data[0] = comando[5];
          uchar_to_int.uc_data[1] = comando[6];
          uchar_to_int.uc_data[2] = comando[7];
          uchar_to_int.uc_data[3] = comando[8];
	  pwm_motor_der = uchar_to_int.i_data;

          xSemaphoreGive( xSemaphorePWMs );
	}
          break;

        case CMD_GET_PWM:
          if((xSemaphoreTake( xSemaphorePWMs, ( portTickType ) portMAX_DELAY ) == pdTRUE )){	
            // PWM del Motor izquierdo
            send_int_data[0] = \
                             loop_motor_l.zero_offset - pwm_get_value(PWM_MATCH5);
            // PWM del Motor derecho
            send_int_data[1] = \
                             loop_motor_r.zero_offset - pwm_get_value(PWM_MATCH2);                  
            send_packet((char*) send_int_data, 8,UART0);
	    xSemaphoreGive(xSemaphorePWMs);
          }
          break;
	

	/****************************************** 
	/ Comandos de pedido de cuentas de encoder
	*******************************************/
        case CMD_GET_ENC_COUNTER:
	  if( xSemaphoreTake( xSemaphoreEncCountTemp, ( portTickType ) portMAX_DELAY ) == pdTRUE ){
	    send_int_data[0] = encoder_l.count_temp;    // Cuenta del Encoder1
            send_int_data[1] = encoder_r.count_temp;    // Cuenta del Encoder2
            send_packet((char*) send_int_data, 8,UART0);
	    xSemaphoreGive(xSemaphoreEncCountTemp);
	  }
          break;

	/********************************************************************* 
	/ Comandos de pedido de velocidades y distancias recorridas de ruedas
	**********************************************************************/
        case CMD_GET_WHEEL_V:
          if( xSemaphoreTake( xSemaphoreMotorLinearSpeed, ( portTickType ) portMAX_DELAY ) == pdTRUE ){
            send_float_data[0] = loop_motor_l.linear_speed;  // velocidad motor1
            send_float_data[1] = loop_motor_r.linear_speed;  // velocidad motor2
            send_packet((char*) send_float_data, 8,UART0);
	    xSemaphoreGive(xSemaphoreMotorLinearSpeed);
	  }
          break;

        case CMD_GET_WHEEL_W:
          if( xSemaphoreTake( xSemaphoreMotorAngularSpeed, ( portTickType ) portMAX_DELAY ) == pdTRUE ){
            send_float_data[0] = loop_motor_l.angular_speed; 
            send_float_data[1] = loop_motor_r.angular_speed; 
            send_packet((char*) send_float_data, 8,UART0);
	    xSemaphoreGive(xSemaphoreMotorAngularSpeed);
	  }
          break;

        case CMD_GET_WHEEL_D:
          if( xSemaphoreTake( xSemaphoreWheelDist, ( portTickType ) portMAX_DELAY ) == pdTRUE ){
            send_float_data[0] = wheel_distance_left;  
            send_float_data[1] = wheel_distance_right;  
            send_packet((char*) send_float_data, 8,UART0);
            xSemaphoreGive(xSemaphoreWheelDist);  
	  }
        break;

	/************************************* 
	/ Comandos de habilitación de motores
	**************************************/
        case CMD_ENABLE_MOTOR:
    	  if((xSemaphoreTake( xSemaphoreLoopStatus, ( portTickType ) portMAX_DELAY ) == pdTRUE )){
    	    // Inicializacion de los PID del motor derecho, izquierdo y lazo vw
 	    pid_init ( &v_pid,  v_pid.Kp,  v_pid.Ki,  v_pid.Kd);
 	    pid_init (&vw_pid, vw_pid.Kp, vw_pid.Ki, vw_pid.Kd);
            xSemaphoreGive( xSemaphoreLoopStatus ); 
	  }
          // Seteo en 0 el pin P0.22 ---> Salida a Llave H 1 Habilitada 
          gpio_set(MOTOR1_ENABLE_PIN, GPIO_OFF);   
          // Seteo en 0 el pin P0.27 ---> Salida a Llave H 2 Habilitada          
          gpio_set(MOTOR2_ENABLE_PIN, GPIO_OFF);   
          break;

        case CMD_DISABLE_MOTOR:
    	  if((xSemaphoreTake( xSemaphoreLoopStatus, ( portTickType ) portMAX_DELAY ) == pdTRUE )){
    	    // Inicializacion de los PID del motor derecho, izquierdo y lazo vw
 	    pid_init ( &v_pid,  v_pid.Kp,  v_pid.Ki,  v_pid.Kd);
 	    pid_init (&vw_pid, vw_pid.Kp, vw_pid.Ki, vw_pid.Kd);
 	    //loop_status = OPEN_LOOP;    
            xSemaphoreGive( xSemaphoreLoopStatus ); 
	  }
          // Seteo en 1 el pin P0.22 ---> Salida a Llave H 1 Deshabilitada
          gpio_set(MOTOR1_ENABLE_PIN, GPIO_ON);            
          // Seteo en 1 el pin P0.27 ---> Salida a Llave H 2 Deshabilitada
          gpio_set(MOTOR2_ENABLE_PIN, GPIO_ON);    
          break;


	/******************************************** 
	/ Comandos de seteo y pedido de modo de lazo
	*********************************************/
        case CMD_SET_OPEN_LOOP_MODE:
    	  if((xSemaphoreTake( xSemaphoreLoopStatus, ( portTickType ) portMAX_DELAY ) == pdTRUE )){
 	    loop_status = OPEN_LOOP;    
            xSemaphoreGive( xSemaphoreLoopStatus ); 
	  }
	  break;

        case CMD_SET_CLOSE_LOOP_MODE:
    	  if((xSemaphoreTake( xSemaphoreLoopStatus, ( portTickType ) portMAX_DELAY ) == pdTRUE )){
    	    // Inicializacion de los PID del motor derecho, izquierdo y lazo vw
 	    pid_init ( &v_pid,  v_pid.Kp,  v_pid.Ki,  v_pid.Kd);
 	    pid_init (&vw_pid, vw_pid.Kp, vw_pid.Ki, vw_pid.Kd);
 	    loop_status = CLOSE_LOOP;    
            xSemaphoreGive( xSemaphoreLoopStatus ); 
	  }
          break;

        case CMD_GET_LOOP_MODE:
          send_packet(&loop_status, 1,UART0);
          break;

	/***************************************************** 
	/ Comandos de seteo y pedido de variables kinemáticas
	******************************************************/
        case CMD_SET_KINEMATIC:
    	  if((xSemaphoreTake( xSemaphoreKinematic, ( portTickType ) portMAX_DELAY ) == pdTRUE)){

            uchar_to_float.uc_data[0] = comando[1]; 
            uchar_to_float.uc_data[1] = comando[2]; 
            uchar_to_float.uc_data[2] = comando[3]; 
            uchar_to_float.uc_data[3] = comando[4]; 
            ConfigParams.wheelbase = uchar_to_float.f_data;

            uchar_to_float.uc_data[0] = comando[5]; 
            uchar_to_float.uc_data[1] = comando[6]; 
            uchar_to_float.uc_data[2] = comando[7]; 
            uchar_to_float.uc_data[3] = comando[8]; 
            ConfigParams.wheel_radious = uchar_to_float.f_data;

            loop_motor_l.feedback_cte = ConfigParams.wheel_radious;
            loop_motor_r.feedback_cte = ConfigParams.wheel_radious;

            writeConfigToFlash();

            xSemaphoreGive( xSemaphoreKinematic );
	  } 
          break;

        case CMD_GET_KINEMATIC:
          send_float_data[0] = ConfigParams.wheelbase;
          send_float_data[1] = ConfigParams.wheel_radious;
          send_packet((char*) send_float_data, 8,UART0);
          break;

        case CMD_GET_BODY_GEOM:
          send_float_data[0] = ROMAA_BODY_WIDTH;
          send_float_data[1] = ROMAA_BODY_LENGHT;
          send_float_data[2] = ROMAA_BODY_HIGH;
          send_packet( (char*) send_float_data,12,UART0);
          break;


	/*************************************** 
	/ Comandos de seteo y pedido de logging
	****************************************/
        case CMD_LOG_STOP:
          log_flags  = LOG_STOP_FLAG;
          logging_send_flag = FALSE;
          vTaskSuspend(xLoggingHandle);
          break;

        case CMD_LOG_WHEEL_V:
          log_flags  = LOG_WHEEL_V_FLAG ;
          vTaskResume(xLoggingHandle);
          logging_send_flag = FALSE;
          break;

        case CMD_LOG_WHEEL_W:
          log_flags = LOG_WHEEL_W_FLAG;
          vTaskResume(xLoggingHandle);
          logging_send_flag = FALSE;
          break;

        case CMD_LOG_WHEEL_D:
          log_flags = LOG_WHEEL_D_FLAG;
          vTaskResume(xLoggingHandle);
          logging_send_flag = FALSE;
          break;

        case CMD_LOG_ENC_COUNTER:
          log_flags = LOG_ENC_COUNTER_FLAG;
          vTaskResume(xLoggingHandle);
          logging_send_flag = FALSE;
          break;

        case CMD_LOG_PWM:
          log_flags = LOG_PWM_FLAG;
          vTaskResume(xLoggingHandle);
          logging_send_flag = FALSE;
          break;

        case CMD_LOG_VW:
          log_flags = LOG_VW_FLAG;
          vTaskResume(xLoggingHandle);
          logging_send_flag = FALSE;
          break;

        case CMD_LOG_ODOM:
          log_flags = LOG_ODOM_FLAG;
          vTaskResume(xLoggingHandle);
          logging_send_flag = FALSE;
          break;


	/************************************************** 
	/ Comandos de pedido y seteo de chequeo de batería
	***************************************************/
	case CMD_GET_BATTERY_LEVEL: 
	  send_packet((char*) &VBattery, 4,UART0);
          break;
	case CMD_GET_ADC_VALUE: 
	  send_packet((char*) &adcValue, 2,UART0);
          break;
	case CMD_SET_ADC_CONV_CTE: 
	  uchar_to_float.uc_data[0] = comando[1]; 
          uchar_to_float.uc_data[1] = comando[2]; 
          uchar_to_float.uc_data[2] = comando[3]; 
          uchar_to_float.uc_data[3] = comando[4]; 
          ConfigParams.ADCConvConstant = uchar_to_float.f_data;

          writeConfigToFlash();

          break;
	case CMD_SET_BATTERY_H_THR: 
	  uchar_to_float.uc_data[0] = comando[1]; 
          uchar_to_float.uc_data[1] = comando[2]; 
          uchar_to_float.uc_data[2] = comando[3]; 
          uchar_to_float.uc_data[3] = comando[4]; 
          ConfigParams.VBatteryHigh = uchar_to_float.f_data;

          writeConfigToFlash();

          break;
	case CMD_SET_BATTERY_L_THR: 
	  uchar_to_float.uc_data[0] = comando[1]; 
          uchar_to_float.uc_data[1] = comando[2]; 
          uchar_to_float.uc_data[2] = comando[3]; 
          uchar_to_float.uc_data[3] = comando[4]; 
          ConfigParams.VBatteryLow = uchar_to_float.f_data;

          writeConfigToFlash();

          break;
	case CMD_GET_BATTERY_H_THR: 
	  send_packet((char*) &ConfigParams.VBatteryHigh, 4,UART0);
          break;
	case CMD_GET_BATTERY_L_THR: 
	  send_packet((char*) &ConfigParams.VBatteryLow,  4,UART0);
          break;
	case CMD_ENABLE_CHECK_BATTERY: 
	  ConfigParams.checkBattery=TRUE;

          writeConfigToFlash();

          break;
        case CMD_DISABLE_CHECK_BATTERY: 
	  ConfigParams.checkBattery=FALSE;

          writeConfigToFlash();

          break;
        default:
          break;
        }
    }
    else{
      if(ContinuousMode == 0x01){
        timeOutComm++;
        // Si se cumple el timeout de ausencia de comandos, seteo velocidad cero
        if(timeOutComm >= (float)(MAX_TIMEOUT_COMM/(float)ConfigParams.communication_ms)){
          v_ref = 0;         
          w_ref = 0;
          timeOutComm=0;
        }
      }

    }



    
    /*************************************** 
    / Envío de datos de logging
    ****************************************/
    if( (log_flags != LOG_STOP_FLAG ) && (logging_send_flag == TRUE)){
      switch(log_flags){
        case LOG_WHEEL_V_FLAG:
        case LOG_WHEEL_W_FLAG:
        case LOG_WHEEL_D_FLAG:
        case LOG_VW_FLAG:
          send_packet((char*) log_float_data, 8,UART0);
          break;
        case LOG_ODOM_FLAG:
          send_packet((char*) log_float_data, 12,UART0);
          break;
        case LOG_ENC_COUNTER_FLAG:
        case LOG_PWM_FLAG:
          send_packet((char*) log_int_data, 8,UART0);
          break;
      }
      logging_send_flag = FALSE;   
    }



    vTaskDelay( xDelay );
  }        
}




/******************************* Functions ***********************************/
/* ------------------------- prvSetupHardware ------------------------------ */

static void prvSetupHardware( void ) {
  struct pll_config PLL;
  
  #ifdef RUN_FROM_RAM
    // Remapea los vectores de interrupcion a la RAM si estamos corriendo desde 
    // RAM
    SCB_MEMMAP = 2;
  #endif

  // Configura los pines RS232. Todos los demas pines se mantienen en su valor 
  // por defecto, 0.
  PCB_PINSEL0 |= mainTX_ENABLE;
  PCB_PINSEL0 |= mainRX_ENABLE;

  // Setea todo los GPIO como salida, menos el P0.14 (BSL) y los pines JTAG.
  IODIR0 = ~( mainP0_14 + mainJTAG_PORT );

  //Seteo del PLL
  PLL.msel = mainPLL_MUL_4;
  PLL.psel = mainPLL_DIV_1;
  PLL.control = mainPLL_ENABLE;
  pll_configuration(&PLL);
  PLL.control = mainPLL_CONNECT;
  pll_configuration(&PLL);


  // Setea y enciende la MAM. Tres ciclos de accesos son usado, debido al 
  // rapido PLL usado. 
  MAM_MAMTIM = mainMAM_TIM_3;
  MAM_MAMCR = mainMAM_MODE_FULL;

  // Setea el bus periferico para que sea el mismo que la salida del PLL
  SCB_VPBDIV = mainBUS_CLK_FULL;


  /* Inicializa las variables de los PID, motores, periodos de tareas, 
     odometria, entre otros */

  /* --------- Configuracion de Fases del encoder_l, como GPIO entrada------- */
  gpio_init(GPIO_PIN0_23, GPIO_IN);         // Fase A
  gpio_init(GPIO_PIN0_16, GPIO_IN);         // Fase B

  /* --------- Configuracion de Fases del encoder_r, como GPIO entrada ------ */
  gpio_init(GPIO_PIN0_30, GPIO_IN);         // Fase B
  gpio_init(GPIO_PIN0_25, GPIO_IN);         // Fase A 

  /* -- Inicializacion PWM5 (P0.21) para MOTOR1 y PWM2 (P0.7) para MOTOR2 -- */
  pwm_set_on(PWM_MATCH5, PWM_NONE);
  pwm_set_value(PWM_MATCH5, ZERO_OFFSET);    // PWM5 aproximadamente al 50% 
                                             // (ciclo de trabajo).
  pwm_set_on(PWM_MATCH2, PWM_NONE);
  pwm_set_value(PWM_MATCH2, ZERO_OFFSET);    // PWM2 aproximadamente al 50%
                                             // (ciclo de trabajo).
  // MATCH0 es común a todos los PWM y configura el período global
  pwm_set_on(PWM_MATCH0, PWM_RESET);
  pwm_set_value(PWM_MATCH0, 3000);           // Frecuencia de trabajo 
                                             // aproximadamente 20Khz.



  /* ---------------- Configuracion de Capture del Timer0 ------------------ */
  // Configura el P0.2 como CAP0.0, FASE A del Encoder 1
  capture_init(TMR_CAP00_P02, TMR_CAP_RISING_EDGE |TMR_CAP_EVENT,2); 
  // Configura el P0.29 como CAP0.3, FASE A del Encoder 2  
  capture_init(TMR_CAP03_P029,TMR_CAP_RISING_EDGE |TMR_CAP_EVENT,2); 
  // Timer Counter y Preescaler Counter habilitatados para contar (TIMER0)
  TMR_TIMER0_ENABLE();                                 

  /* ---------------- Configuracion de llaves H ------------------ */
  // Seteo como salida para los pines de las llaves H
  gpio_init(MOTOR1_ENABLE_PIN, GPIO_OUT);
  gpio_init(MOTOR2_ENABLE_PIN, GPIO_OUT);
  //Llaves H DESACTIVADAS por defecto
  gpio_set(MOTOR1_ENABLE_PIN, GPIO_ON);  // Seteo en 1 el pin P0.22 ---> Salida a Llave H 1 
  gpio_set(MOTOR2_ENABLE_PIN, GPIO_ON);  // Seteo en 1 el pin P0.27 ---> Salida a Llave H 2

  /* --------------------- Configuracion de la UART0 ----------------------- */
  UART_Init(UART0);
  UART_BaudRateConfig(UART0, UART_BAUD(115200));
  UART_ModeConfig(UART0, UART_8bits | UART_OffParity | UART_1stop);
  
  UART_FifoConfig(UART0, \
         UART_ON_Fifo|UART_Fifo_Rx_Reset|UART_Fifo_Tx_Reset|UART_Rx_14_Tlevel);
 
  UART_Interrupt_priority(UART0, 4);
  UART_Enable_Interrupt(UART0, UART_RDA_CTI);

  /* ----------- Inicialización leds indicadores de las tareas --------------- */  
  //TaskRTOSAlive
  gpio_init(GPIO_PIN0_5, GPIO_OUT);
  gpio_set(GPIO_PIN0_5 , GPIO_OFF);
  gpio_init(GPIO_PIN0_8, GPIO_OUT);
  gpio_set(GPIO_PIN0_8 , GPIO_OFF);
  //ADC Battery
  gpio_init(GPIO_PIN0_6, GPIO_OUT);
  gpio_set(GPIO_PIN0_6 , GPIO_OFF); 
  //TaskPIDLoop
  gpio_init(GPIO_PIN0_9, GPIO_OUT);
  gpio_set(GPIO_PIN0_9 , GPIO_OFF);
  //TaskOdometry
  gpio_init(GPIO_PIN0_10, GPIO_OUT);
  gpio_set(GPIO_PIN0_10 , GPIO_OFF);
  //TaskSerialCommunication or TaskLogging
  gpio_init(GPIO_PIN0_11, GPIO_OUT);
  gpio_set(GPIO_PIN0_11 , GPIO_OFF);

  /* --------------------- Configuracion del ADC ----------------------- */
  adc_init(AN1_P028,ADC_CLOCK_DIVISOR, NULL);
   
}




/* ------------------------------ init_all --------------------------------- */
/* Inicialización y creación de todos los recursos de software a usar */

void init_all(void)
{

  // Preparo la función IAP
  iap_entry=(IAP) IAP_LOCATION;
  p_ConfigParams = FLASH_SECTOR_15;

  // Reseteo de variables
  reset_all();

  //Callbacks de interrupciones de los módulos capture
  callbacks_timer0_capture0 = c_timer0_capture0;
  callbacks_timer0_capture3 = c_timer0_capture3;
  
  /* -------------- Inicio comunicación serie --------------- */
  com_init(UART0);

  // Creación de semáforos para comunicación entre tareas
  xSemaphorePWMs 	   	= xSemaphoreCreateMutex();
  xSemaphoreVWRef 	   	= xSemaphoreCreateMutex();
  xSemaphoreMotorPIDParams 	= xSemaphoreCreateMutex();
  xSemaphoreVWPIDParams    	= xSemaphoreCreateMutex();
  xSemaphoreLoopStatus     	= xSemaphoreCreateMutex();
  xSemaphoreEncCountTemp  	= xSemaphoreCreateMutex();
  xSemaphoreWheelDist  	   	= xSemaphoreCreateMutex();
  xSemaphoreXYTheta  	   	= xSemaphoreCreateMutex();
  xSemaphoreMotorLinearSpeed  	= xSemaphoreCreateMutex();
  xSemaphoreMotorAngularSpeed  	= xSemaphoreCreateMutex();
  xSemaphoreKinematic	  	= xSemaphoreCreateMutex();

}


/* ------------------------------ reset_all --------------------------------- */
/* Inicialización de variables y estructuras usadas  */

void reset_all(void)
{
  unsigned int flashBlank; 

  //Velocidades angulares
  w_ref   = 0;
  w_out   = 0;
  v_l_out = 0;
  v_r_out = 0;
  v_ref   = 0;

  //Modo de lazo
  loop_status = OPEN_LOOP; 

  //Motores
  pwm_motor_izq = 0;
  pwm_motor_der = 0;

  // Modo de comandos de comunicación
  ContinuousMode = CONTINUOUS_MODE;

  //Flags de Logging
  logging_send_flag = FALSE;
  log_flags = LOG_STOP_FLAG;

  //Odometría
  center_distance=0;
  center_phi=0; 
  wheel_distance_left=0;
  wheel_distance_right=0;
  x = 0;
  y = 0;
  theta = 0;

  //Encoders
  encoder_init( &encoder_l );
  encoder_init( &encoder_r );

  // Nivel de batería
  adcValue=0;

  flashBlank = checkBlankFlash();
  if(flashBlank == 0){
    ConfigParams.loop_pid_ms      = DEFAULT_T_PID_LOOP;
    ConfigParams.logging_ms       = DEFAULT_T_LOGGING;
    ConfigParams.odometry_ms      = DEFAULT_T_ODOM;
    ConfigParams.communication_ms = DEFAULT_T_COMM; 
    ConfigParams.rtos_alive_ms    = DEFAULT_T_RTOS_ALIVE;  

    //Cinemática - Geometría
    ConfigParams.wheelbase     = NOM_WHEELBASE;
    ConfigParams.wheel_radious = NOM_WHEEL_RADIOUS;

    // Nivel de batería
    ConfigParams.VBatteryHigh = BATTERY_LEVEL_HIGH;
    ConfigParams.VBatteryLow  = BATTERY_LEVEL_LOW;
    ConfigParams.checkBattery = FALSE;
    ConfigParams.ADCConvConstant = ADC_CONV_CTE;

    ConfigParams.v_pid_kp  = V_PID_KP;
    ConfigParams.v_pid_ki  = V_PID_KI;
    ConfigParams.v_pid_kd  = V_PID_KD;
    ConfigParams.vw_pid_kp = VW_PID_KP;
    ConfigParams.vw_pid_ki = VW_PID_KI;
    ConfigParams.vw_pid_kd = VW_PID_KD;

  }
  else{
    // Leo los datos de configuración de la flash
    readConfigFromFlash();
  }

    // Inicializacion de los PID del motor derecho, izquierdo y lazo vw
    pid_init(  &v_pid, ConfigParams.v_pid_kp,   ConfigParams.v_pid_ki,  ConfigParams.v_pid_kd  );
    pid_init( &vw_pid, ConfigParams.vw_pid_kp,  ConfigParams.vw_pid_ki, ConfigParams.vw_pid_kd );

    // Inicializacion de las variables de los motores 
    loop_motor_init( &loop_motor_l, (int)ZERO_OFFSET, (float)ConfigParams.wheel_radious );
    loop_motor_init( &loop_motor_r, (int)ZERO_OFFSET, (float)ConfigParams.wheel_radious );

}




/* ------------------------- checkBlankFlash ------------------------------- */
/*           Leo los parámetros de configuración de la flash                 */
unsigned int checkBlankFlash(){
  unsigned int temp;

  memset(commandIAP, 0x00, sizeof(commandIAP));
  memset(resultIAP,  0x00, sizeof(resultIAP));
  // Preparamos el sector para esribir
  commandIAP[0]=53;
  commandIAP[1]=15;   // Sector 15 (ante penultimo sector)
  commandIAP[2]=15;

  portENTER_CRITICAL();
  iap_entry (commandIAP, resultIAP);
  portEXIT_CRITICAL();

  return resultIAP[0];

}


/* ----------------------- readConfigFromFlash ----------------------------- */
/*           Leo los parámetros de configuración de la flash                 */
void readConfigFromFlash(void){

  memset(&ConfigParams, 0x00, sizeof(ConfigParams));
  memcpy(&ConfigParams, p_ConfigParams, sizeof(ConfigParams));

  return;

}


/* ------------------------ writeConfigToFlash ----------------------------- */
/*          Escribo los parámetros de configuración a la flash               */
void writeConfigToFlash(void){

  // Preparamos el sector para borrar
  memset(commandIAP, 0x00, sizeof(commandIAP));
  memset(resultIAP,  0x00, sizeof(resultIAP));
  commandIAP[0]=PREPAIR_WRITE;
  commandIAP[1]=15;   // Sector 15 (ante penultimo sector)
  commandIAP[2]=15;
  portENTER_CRITICAL();
  iap_entry (commandIAP, resultIAP);
  portEXIT_CRITICAL();

  // Borramos el sector
  memset(commandIAP, 0x00, sizeof(commandIAP));
  memset(resultIAP,  0x00, sizeof(resultIAP));
  commandIAP[0]=ERASE_SECTORS;
  commandIAP[1]=15;   // Sector 15 (ante penultimo sector)
  commandIAP[2]=15;
  commandIAP[3]=MCU_CLK_KHZ;     // CCLK en KHz
  portENTER_CRITICAL();
  iap_entry (commandIAP, resultIAP);
  portEXIT_CRITICAL();

  // Preparamos el sector para esribir
  memset(commandIAP, 0x00, sizeof(commandIAP));
  memset(resultIAP,  0x00, sizeof(resultIAP));
  commandIAP[0]=PREPAIR_WRITE;
  commandIAP[1]=15;   // Sector 15 (ante penultimo sector)
  commandIAP[2]=15;
  portENTER_CRITICAL();
  iap_entry (commandIAP, resultIAP);
  portEXIT_CRITICAL();
 
  // Escribimos el sector
  memset(commandIAP, 0x00, sizeof(commandIAP));
  memset(resultIAP,  0x00, sizeof(resultIAP));
  commandIAP[0]=COPY_RAM_TO_FLASH;
  commandIAP[1]=FLASH_SECTOR_15;  // inicio del sector 15
  commandIAP[2]=&ConfigParams;    
  commandIAP[3]=512;              // Bytes a escribir           
  commandIAP[4]=MCU_CLK_KHZ;      // CCLK en KHz
  portENTER_CRITICAL();
  iap_entry (commandIAP, resultIAP);
  portEXIT_CRITICAL();

  return;

}









/* ------------------------- loop_motor_init ------------------------------- */
/* Inicializacion de todas las variables de los motores y su lazo de control.*/
 
void loop_motor_init( motor_loop_t* loop_motor, int zero_offset, \
                      float feedback_cte ) {
  
  loop_motor->angular_speed   = 0;
  loop_motor->linear_speed = 0;
  loop_motor->zero_offset = zero_offset;  // se setea en 1500, ciclo de trabajo medio en el PWM,(Cero en las llaves H)
  //loop_motor->reference = 0;
  loop_motor->feedback_cte = feedback_cte;
}



/******************************* Callbacks ***********************************/

/* ------------------------- c_timer0_capture0 ----------------------------- */
/* Funcion de callback llamada por la interrupcion del capture 0 del timer 0.*/

void c_timer0_capture0(void) {
  // Si no habia lectura previa, espera la siguiente
  if (encoder_l.edge_previous == 0)    
  {     
    encoder_l.edge_previous = capture_get(TMR_CAP00_P02);
  }
  else
  {
    encoder_l.edge_current = capture_get(TMR_CAP00_P02);

    // Si encoder_l.edge_current es menor, hubo un desborde en el TC
    if (encoder_l.edge_current < encoder_l.edge_previous) 
    {
      // El periodo es igual a lo que faltaba para quedesborde 
      // (se obtiene haciendo el complemento a 2) mas el flanco actual
      encoder_l.period_pclk = \
                         encoder_l.edge_current + (~encoder_l.edge_previous) + 1;  
    }    
    else    // Si no desbordo, el periodo es simplemente la diferencia                     
    {
      encoder_l.period_pclk = encoder_l.edge_current - encoder_l.edge_previous;     
    }

    // Acutaliza flanco previo para prox flanco
    #ifndef ROMAA_II
      encoder_l.edge_previous = encoder_l.edge_current;
      encoder_update(GPIO_PIN0_23, GPIO_PIN0_16, &encoder_l.count, \
                     &encoder_l.period_pclk, ROMAA_MODE);
    #else
      encoder_l.edge_previous = encoder_l.edge_current;
      encoder_update(GPIO_PIN0_23, GPIO_PIN0_16, &encoder_l.count, \
                     &encoder_l.period_pclk, ROMAA_II_MODE);
    #endif

  }
  encoder_l.recent_pulse = 0; // Limpia la variable de velocidad cero
}



/* ------------------------- c_timer0_capture3 ----------------------------- */
/* Funcion de callback llamada por la interrupcion del capture 3 del timer 0.*/

void c_timer0_capture3(void) {
  // Si no habia lectura previa, espera la siguiente
  if (encoder_r.edge_previous == 0)    
  {
    encoder_r.edge_previous = capture_get(TMR_CAP03_P029);
  }
  else
  {
    encoder_r.edge_current = capture_get(TMR_CAP03_P029);

    // Si encoder_r.edge_current es menor, hubo un desborde en el TC
    if (encoder_r.edge_current < encoder_r.edge_previous)
    {
      // El periodo es igual a lo que faltaba para que desborde 
      // (se obtiene haciendo el complemento a 2) mas el flanco actual.
      encoder_r.period_pclk = \
                         encoder_r.edge_current + (~encoder_r.edge_previous) + 1;  
    }                                                                             
    else   // Si no desbordo, el periodo es simplemente la diferencia  
    {
      encoder_r.period_pclk = encoder_r.edge_current - encoder_r.edge_previous;  
    }
    encoder_r.edge_previous = encoder_r.edge_current;

    // Se cambia el orden de las Fases debido a que los motores estan opuestos
    #ifndef ROMAA_II
      encoder_update(GPIO_PIN0_25, GPIO_PIN0_30, &encoder_r.count, \
                     &encoder_r.period_pclk, ROMAA_MODE);
    #else
      encoder_update(GPIO_PIN0_25, GPIO_PIN0_30, &encoder_r.count, \
                     &encoder_r.period_pclk, ROMAA_II_MODE);
    #endif
  }
  encoder_r.recent_pulse = 0;          // Limpia la variable de velocidad cero     
}





