BeRTOS
|
00001 00039 #include "pid_control.h" 00040 00041 #include "cfg/cfg_pid.h" 00042 00043 // Define logging setting (for cfg/log.h module). 00044 #define LOG_LEVEL PID_LOG_LEVEL 00045 #define LOG_VERBOSITY PID_LOG_FORMAT 00046 00047 #include <cfg/log.h> 00048 #include <cfg/debug.h> 00049 00053 piddata_t pid_control_update(PidContext *pid_ctx, piddata_t target, piddata_t curr_pos) 00054 { 00055 piddata_t P; 00056 piddata_t I; 00057 piddata_t D; 00058 piddata_t err; 00059 00060 //Compute current error. 00061 err = target - curr_pos; 00062 00063 /* 00064 * Compute Proportional contribute 00065 */ 00066 P = err * pid_ctx->cfg->kp; 00067 00068 //Update integral state error 00069 pid_ctx->i_state += err; 00070 00071 //Clamp integral state between i_min and i_max 00072 pid_ctx->i_state = MINMAX(pid_ctx->cfg->i_min, pid_ctx->i_state, pid_ctx->cfg->i_max); 00073 00074 /* 00075 * Compute Integral contribute 00076 * 00077 * note: for computing the integral contribute we use a sample period in seconds 00078 * and so we divide sample_period in microsenconds for 1000. 00079 */ 00080 I = pid_ctx->i_state * pid_ctx->cfg->ki * ((piddata_t)pid_ctx->cfg->sample_period / 1000); 00081 00082 00083 /* 00084 * Compute derivative contribute 00085 */ 00086 D = (err - pid_ctx->prev_err) * pid_ctx->cfg->kd / ((piddata_t)pid_ctx->cfg->sample_period / 1000); 00087 00088 00089 LOG_INFO("curr_pos[%lf],tgt[%lf],err[%f],P[%f],I[%f],D[%f]", curr_pos, target, err, P, I, D); 00090 00091 00092 //Store the last error value 00093 pid_ctx->prev_err = err; 00094 piddata_t pid = MINMAX(pid_ctx->cfg->out_min, (P + I + D), pid_ctx->cfg->out_max); 00095 00096 LOG_INFO("pid[%lf]",pid); 00097 00098 //Clamp out between out_min and out_max 00099 return pid; 00100 } 00101 00105 void pid_control_init(PidContext *pid_ctx, const PidCfg *pid_cfg) 00106 { 00107 /* 00108 * Init all values of pid control struct 00109 */ 00110 pid_ctx->cfg = pid_cfg; 00111 00112 pid_control_reset(pid_ctx); 00113 00114 } 00115