bertos/algo/pid_control.c

   1 /**
   2  * \file
   3  * <!--
   4  * This file is part of BeRTOS.
   5  *
   6  * Bertos is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, write to the Free Software
  18  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  * As a special exception, you may use this file as part of a free software
  21  * library without restriction.  Specifically, if other files instantiate
  22  * templates or use macros or inline functions from this file, or you compile
  23  * this file and link it with other files to produce an executable, this
  24  * file does not by itself cause the resulting executable to be covered by
  25  * the GNU General Public License.  This exception does not however
  26  * invalidate any other reasons why the executable file might be covered by
  27  * the GNU General Public License.
  28  *
  29  * Copyright 2008 Develer S.r.l. (http://www.develer.com/)
  30  * -->
  31  *
  32  *
  33  * \brief Proportional, integral, derivative controller (PID controller) (implementation)
  34  *
  35  *
  36  * \author Daniele Basile <asterix@develer.com>
  37  */
  38
  39 #include "pid_control.h"
  40
  41 #include "cfg/cfg_pid.h"
  42
  43 // Define logging setting (for cfg/log.h module).
  44 #define LOG_LEVEL         PID_LOG_LEVEL
  45 #define LOG_VERBOSITY     PID_LOG_FORMAT
  46
  47 #include <cfg/log.h>
  48 #include <cfg/debug.h>
  49
  50 /**
  51  * Compute next value for reaching \a target point.
  52  */
  53 piddata_t pid_control_update(PidContext *pid_ctx, piddata_t target, piddata_t curr_pos)
  54 {
  55         piddata_t P;
  56         piddata_t I;
  57         piddata_t D;
  58         piddata_t err;
  59
  60         //Compute current error.
  61         err = target - curr_pos;
  62
  63         /*
  64          * Compute Proportional contribute
  65          */
  66         P = err * pid_ctx->cfg->kp;
  67
  68         //Update integral state error
  69         pid_ctx->i_state += err;
  70
  71         //Clamp integral state between i_min and i_max
  72         pid_ctx->i_state  = MINMAX(pid_ctx->cfg->i_min, pid_ctx->i_state, pid_ctx->cfg->i_max);
  73
  74         /*
  75          * Compute Integral contribute
  76          *
  77          * note: for computing the integral contribute we use a sample period in seconds
  78          * and so we divide sample_period in microsenconds for 1000.
  79          */
  80         I = pid_ctx->i_state * pid_ctx->cfg->ki * ((piddata_t)pid_ctx->cfg->sample_period / 1000);
  81
  82
  83         /*
  84          * Compute derivative contribute
  85          */
  86         D = (err - pid_ctx->prev_err) * pid_ctx->cfg->kd / ((piddata_t)pid_ctx->cfg->sample_period / 1000);
  87
  88
  89         LOG_INFO("curr_pos[%lf],tgt[%lf],err[%f],P[%f],I[%f],D[%f]", curr_pos, target, err, P, I, D);
  90
  91
  92         //Store the last error value
  93         pid_ctx->prev_err = err;
  94         piddata_t pid = MINMAX(pid_ctx->cfg->out_min, (P + I + D), pid_ctx->cfg->out_max);
  95
  96         LOG_INFO("pid[%lf]",pid);
  97
  98         //Clamp out between out_min and out_max
  99         return pid;
 100 }
 101
 102 /**
 103  * Init PID control.
 104  */
 105 void pid_control_init(PidContext *pid_ctx, const PidCfg *pid_cfg)
 106 {
 107         /*
 108          * Init all values of pid control struct
 109          */
 110         pid_ctx->cfg = pid_cfg;
 111
 112         pid_control_reset(pid_ctx);
 113
 114 }
 115