00001 /*This file has been prepared for Doxygen automatic documentation generation.*/ 00023 #include "pid.h" 00024 #include "stdint.h" 00025 00035 void pid_Init(int16_t p_factor, int16_t i_factor, int16_t d_factor, struct PID_DATA *pid) 00036 // Set up PID controller parameters 00037 { 00038 // Start values for PID controller 00039 pid->sumError = 0; 00040 pid->lastProcessValue = 0; 00041 // Tuning constants for PID loop 00042 pid->P_Factor = p_factor; 00043 pid->I_Factor = i_factor; 00044 pid->D_Factor = d_factor; 00045 // Limits to avoid overflow 00046 pid->maxError = MAX_INT / (pid->P_Factor + 1); 00047 pid->maxSumError = MAX_I_TERM / (pid->I_Factor + 1); 00048 } 00049 00050 00059 int16_t pid_Controller(int16_t setPoint, int16_t processValue, struct PID_DATA *pid_st) 00060 { 00061 int16_t error, p_term, d_term; 00062 int32_t i_term, ret, temp; 00063 00064 error = setPoint - processValue; 00065 00066 // Calculate Pterm and limit error overflow 00067 if (error > pid_st->maxError){ 00068 p_term = MAX_INT; 00069 } 00070 else if (error < -pid_st->maxError){ 00071 p_term = -MAX_INT; 00072 } 00073 else{ 00074 p_term = pid_st->P_Factor * error; 00075 } 00076 00077 // Calculate Iterm and limit integral runaway 00078 temp = pid_st->sumError + error; 00079 if(temp > pid_st->maxSumError){ 00080 i_term = MAX_I_TERM; 00081 pid_st->sumError = pid_st->maxSumError; 00082 } 00083 else if(temp < -pid_st->maxSumError){ 00084 i_term = -MAX_I_TERM; 00085 pid_st->sumError = -pid_st->maxSumError; 00086 } 00087 else{ 00088 pid_st->sumError = temp; 00089 i_term = pid_st->I_Factor * pid_st->sumError; 00090 } 00091 00092 // Calculate Dterm 00093 d_term = pid_st->D_Factor * (pid_st->lastProcessValue - processValue); 00094 00095 pid_st->lastProcessValue = processValue; 00096 00097 ret = (p_term + i_term + d_term) / SCALING_FACTOR; 00098 if(ret > MAX_INT){ 00099 ret = MAX_INT; 00100 } 00101 else if(ret < -MAX_INT){ 00102 ret = -MAX_INT; 00103 } 00104 00105 return((int16_t)ret); 00106 } 00107 00112 void pid_Reset_Integrator(pidData_t *pid_st) 00113 { 00114 pid_st->sumError = 0; 00115 }