/* ###################################################################
**     This component module is generated by Processor Expert. Do not modify it.
**     Filename    : McuPidFloat.h
**     CDE edition : Community
**     Project     : FRDM-K64F_Generator
**     Processor   : MK64FN1M0VLL12
**     Component   : PID_Float
**     Version     : Component 01.007, Driver 01.00, CPU db: 3.00.000
**     Compiler    : GNU C Compiler
**     Date/Time   : 2020-08-14, 06:24, # CodeGen: 679
**     Abstract    :
**
**     Settings    :
**     Contents    :
**         Control - void McuPidFloat_Control(float error, void* *u);
**         Reset   - void McuPidFloat_Reset(void);
**         Set_K   - void McuPidFloat_Set_K(float k);
**         Set_Ti  - void McuPidFloat_Set_Ti(float t);
**         Set_Td  - void McuPidFloat_Set_Td(float t);
**
** * (c) Copyright Carlos Alvarez, 2013-2020
**  * For non-commercial use only.
**  * Web:         https://mcuoneclipse.com
**  * SourceForge: https://sourceforge.net/projects/mcuoneclipse
**  * Git:         https://github.com/ErichStyger/McuOnEclipse_PEx
**  * All rights reserved.
** ###################################################################*/
/*!
** @file McuPidFloat.h
** @version 01.00
** @brief
**
*/         
/*!
**  @addtogroup McuPidFloat_module McuPidFloat module documentation
**  @{
*/         

/* MODULE McuPidFloat. */

#include "McuPidFloat.h"
#define K_VAL		0.1 /* Constante proporcional */
#define TI_VAL		100 /* Tiempo integrativo */
#define TD_VAL		0 /* Tiempo derivativo */
#define T_VAL		0.01 /* Período de control */
#define U_MIN		0 /* Valor de salida mínimo */
#define U_MAX		65000 /* Valor de salida máximo */
#define U_CHG_MAX	0 /* Cambio máximo en cada ciclo de control */
/* Coeficientes del control PID */
#define K1		(McuPidFloat_K*(1+(McuPidFloat_Td/T_VAL)))
#define	K2		(-McuPidFloat_K*(1+((2*McuPidFloat_Td)/T_VAL)-(T_VAL/McuPidFloat_Ti)))
#define	K3		(McuPidFloat_K*(McuPidFloat_Td/T_VAL))

/* Variable para la constante proporcional */
static float McuPidFloat_K = K_VAL;
/* Variable para el tiempo integrativo */
static float McuPidFloat_Ti = TI_VAL;
/* Variable para el tiempo derivativo */
static float McuPidFloat_Td = TD_VAL;
/* Variable que almacena los errores */
float McuPidFloat_error[2] = {0.0, 0.0};

/*
** ===================================================================
**     Method      :  Set_Td (component PID_Float)
**
**     Description :
**         Change the Td parameter.
**     Parameters  :
**         NAME            - DESCRIPTION
**         t               - New Td value.
**     Returns     : Nothing
** ===================================================================
*/
void McuPidFloat_Set_Td(float t)
{
  if(t < 0) {
    return;
  }
  McuPidFloat_Td = t;
}

/*
** ===================================================================
**     Method      :  Set_Ti (component PID_Float)
**
**     Description :
**         Change the Ti parameter.
**     Parameters  :
**         NAME            - DESCRIPTION
**         t               - New Ti value.
**     Returns     : Nothing
** ===================================================================
*/
void McuPidFloat_Set_Ti(float t)
{
  if (t <= 0) {
    return;
  }
  McuPidFloat_Ti = t;
}

/*
** ===================================================================
**     Method      :  Set_K (component PID_Float)
**
**     Description :
**         Change the K parameter.
**     Parameters  :
**         NAME            - DESCRIPTION
**         k               - New K value.
**     Returns     : Nothing
** ===================================================================
*/
void McuPidFloat_Set_K(float k)
{
  if(k < 0) {
    return;
  }
  McuPidFloat_K = k;
}

/*
** ===================================================================
**     Method      :  Reset (component PID_Float)
**
**     Description :
**         Reset the PID.
**     Parameters  : None
**     Returns     : Nothing
** ===================================================================
*/
void McuPidFloat_Reset(void)
{
  McuPidFloat_error[0] = 0.0;
  McuPidFloat_error[1] = 0.0;
}

/*
** ===================================================================
**     Method      :  Control (component PID_Float)
**
**     Description :
**         Control function. Must be called every T time.
**     Parameters  :
**         NAME            - DESCRIPTION
**         error           - Error signal. (Reference -
**                           SystemOutput).
**       * u               - Pointer to the control variable.
**     Returns     : Nothing
** ===================================================================
*/
#ifdef __HIWARE__
  #pragma MESSAGE DISABLE C5900
  #pragma MESSAGE DISABLE C5917
#endif

void McuPidFloat_Control(float error, uint16_t *u)
{
  float ut = 0.0;

  ut = (float) *u;
  ut += K1 * error;
  ut += K2 * McuPidFloat_error[0];
  ut += K3 * McuPidFloat_error[1];

  McuPidFloat_error[1] = McuPidFloat_error[0];
  McuPidFloat_error[0] = error;

  if (ut > U_MAX) {
    *u = (uint16_t) U_MAX;
  } else if (ut < U_MIN) {
    *u = (uint16_t) U_MIN;
  } else {
    *u = (uint16_t) ut;
  }
}
#ifdef __HIWARE__
  #pragma MESSAGE DISABLE C5919
#endif

/* END McuPidFloat. */

/*!
** @}
*/