pid.cpp

Go to the documentation of this file.

/* This file has been prepared for Doxygen automatic documentation generation.*/
// Include PID header
#include "pid.h"
 
void PIDInit(int16_t p_factor, int16_t i_factor, int16_t d_factor, pidData_t *pid)
// Set up PID controller parameters
{
  // Start values for PID controller
  pid->sumError = 0;
  pid->lastProcessValue = 0;
  // Tuning constants for PID loop
  pid->P_Factor = p_factor;
  pid->I_Factor = i_factor;
  pid->D_Factor = d_factor;
  // Limits to avoid overflow
  pid->maxError = MAX_INT / pid->P_Factor;
  pid->maxSumError = MAX_I_TERM / pid->I_Factor;
}
 
uint16_t PIDController(int16_t setPoint, int16_t processValue, pidData_t *pid_st)
{
  int32_t ret;
  int32_t temp;
  int16_t error;
 
  error = setPoint - processValue;
 
  // Calculate "P" term and limit error overflow
  if (error > pid_st->maxError)
  {
    pid_st->p_term = MAX_INT;
  }
  else if (error < -pid_st->maxError)
  {
    pid_st->p_term = -MAX_INT;
  }
  else
  {
    pid_st->p_term = pid_st->P_Factor * error / 1000;
  }
 
  // Calculate "I" term and limit integral runaway
  temp = pid_st->sumError + error;
  if (temp > pid_st->maxSumError)
  {
    pid_st->i_term = MAX_I_TERM;
    pid_st->sumError = pid_st->maxSumError;
  }
  else if (temp < -pid_st->maxSumError)
  {
    pid_st->i_term = -MAX_I_TERM;
    pid_st->sumError = -pid_st->maxSumError;
  }
  else
  {
    pid_st->sumError = temp;
    pid_st->i_term = pid_st->I_Factor * pid_st->sumError / 1000;
  }
 
#if (PID_K_D_ENABLE == TRUE)
  // Calculate "D" term
  pid_st->d_term = pid_st->D_Factor * (pid_st->lastProcessValue - processValue) / 1000;
#endif
 
  pid_st->lastProcessValue = processValue;
 
#if (PID_K_D_ENABLE == TRUE)
  ret = (pid_st->p_term + pid_st->i_term + pid_st->d_term);
#else
  ret = (pid_st->p_term + pid_st->i_term);
#endif
 
#if (SCALING_FACTOR_ENABLED == TRUE)
  ret = ret / SCALING_FACTOR;
#endif
  if (ret > MAX_INT)
  {
    ret = MAX_INT;
  }
  // Since the return type is uint16_t
  else if (ret < 0)
  {
    ret = 0;
  }
 
  return ((uint16_t)ret);
}
 
void PIDResetIntegrator(pidData_t *pid_st)
{
  pid_st->sumError = 0;
  pid_st->p_term = 0;
  pid_st->i_term = 0;
#if (PID_K_D_ENABLE == TRUE)
  pid_st->d_term = 0;
#endif
}