124 lines
3.2 KiB
C
124 lines
3.2 KiB
C
/*This file has been prepared for Doxygen automatic documentation generation.*/
|
|
#include "pid.h"
|
|
#include "stdint.h"
|
|
|
|
void pid_Init(int16_t p_factor, int16_t i_factor, int16_t d_factor, struct PID_DATA *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 + 1);
|
|
pid->maxSumError = MAX_I_TERM / (pid->I_Factor + 1);
|
|
}
|
|
|
|
/*
|
|
int16_t pid_Controller(int16_t setPoint, int16_t processValue, struct PID_DATA *pid_st)
|
|
{
|
|
int16_t error, p_term, d_term;
|
|
int32_t i_term, ret, temp;
|
|
|
|
error = setPoint - processValue;
|
|
|
|
// Calculate Pterm and limit error overflow
|
|
if (error > pid_st->maxError){
|
|
p_term = MAX_INT;
|
|
}
|
|
else if (error < -pid_st->maxError){
|
|
p_term = -MAX_INT;
|
|
}
|
|
else{
|
|
p_term = pid_st->P_Factor * error;
|
|
}
|
|
|
|
// Calculate Iterm and limit integral runaway
|
|
temp = pid_st->sumError + error;
|
|
if(temp > pid_st->maxSumError){
|
|
i_term = MAX_I_TERM;
|
|
pid_st->sumError = pid_st->maxSumError;
|
|
}
|
|
else if(temp < -pid_st->maxSumError){
|
|
i_term = -MAX_I_TERM;
|
|
pid_st->sumError = -pid_st->maxSumError;
|
|
}
|
|
else{
|
|
pid_st->sumError = temp;
|
|
i_term = pid_st->I_Factor * pid_st->sumError;
|
|
}
|
|
|
|
// Calculate Dterm
|
|
d_term = pid_st->D_Factor * (pid_st->lastProcessValue - processValue);
|
|
|
|
pid_st->lastProcessValue = processValue;
|
|
|
|
ret = (p_term + i_term + d_term) / SCALING_FACTOR;
|
|
if(ret > MAX_INT){
|
|
ret = MAX_INT;
|
|
}
|
|
else if(ret < -MAX_INT){
|
|
ret = -MAX_INT;
|
|
}
|
|
|
|
return((int16_t)ret);
|
|
} */
|
|
|
|
int16_t pid_Controller(int16_t setPoint, int16_t processValue, int16_t errorValue, struct PID_DATA *pid_st)
|
|
{
|
|
int16_t error, p_term, d_term;
|
|
int32_t i_term, ret, temp;
|
|
|
|
error = setPoint - processValue;
|
|
|
|
// Calculate Pterm and limit error overflow
|
|
if (error > pid_st->maxError){
|
|
p_term = MAX_INT;
|
|
}
|
|
else if (error < -pid_st->maxError){
|
|
p_term = -MAX_INT;
|
|
}
|
|
else{
|
|
p_term = pid_st->P_Factor * error;
|
|
}
|
|
|
|
// Calculate Iterm and limit integral runaway
|
|
temp = pid_st->sumError + error;
|
|
if(temp > pid_st->maxSumError){
|
|
i_term = MAX_I_TERM;
|
|
pid_st->sumError = pid_st->maxSumError;
|
|
}
|
|
else if(temp < -pid_st->maxSumError){
|
|
i_term = -MAX_I_TERM;
|
|
pid_st->sumError = -pid_st->maxSumError;
|
|
}
|
|
else{
|
|
pid_st->sumError = temp;
|
|
i_term = pid_st->I_Factor * pid_st->sumError;
|
|
}
|
|
|
|
// Calculate Dterm
|
|
d_term = pid_st->D_Factor * (pid_st->lastProcessValue - processValue);
|
|
|
|
pid_st->lastProcessValue = processValue;
|
|
|
|
//ret = (p_term + i_term + d_term + errorValue) / SCALING_FACTOR;
|
|
ret = (p_term + i_term + d_term + errorValue) / SCALING_FACTOR;
|
|
if(ret > (int32_t) MAX_INT){
|
|
ret = MAX_INT;
|
|
}
|
|
else if(ret < (int32_t) -MAX_INT){
|
|
ret = -MAX_INT;
|
|
}
|
|
|
|
return((int16_t)ret);
|
|
}
|
|
|
|
|
|
void pid_Reset_Integrator(pidData_t *pid_st)
|
|
{
|
|
pid_st->sumError = 0;
|
|
} |