BeRTOS
pid_control.c
Go to the documentation of this file.
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