Clean up and using correct configuration.
[bertos.git] / 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  * \version $Id$
36  *
37  * \author Daniele Basile <asterix@develer.com>
38  */
39
40 #include "pid_control.h"
41
42 #include "cfg/cfg_pid.h"
43
44 // Define logging setting (for cfg/log.h module).
45 #define LOG_LEVEL         PID_LOG_LEVEL
46 #define LOG_VERBOSITY     PID_LOG_FORMAT
47
48 #include <cfg/log.h>
49 #include <cfg/debug.h>
50
51 /**
52  * Compute next value for reaching \a target point.
53  */
54 piddata_t pid_control_update(PidContext *pid_ctx, piddata_t target, piddata_t curr_pos)
55 {
56         piddata_t P;
57         piddata_t I;
58         piddata_t D;
59         piddata_t err;
60
61         //Compute current error.
62         err = target - curr_pos;
63
64         /*
65          * Compute Proportional contribute
66          */
67         P = err * pid_ctx->cfg->kp;
68
69         //Update integral state error
70         pid_ctx->i_state += err;
71
72         //Clamp integral state between i_min and i_max
73         pid_ctx->i_state  = MINMAX(pid_ctx->cfg->i_min, pid_ctx->i_state, pid_ctx->cfg->i_max);
74
75         /*
76          * Compute Integral contribute
77          *
78          * note: for computing the integral contribute we use a sample period in seconds
79          * and so we divide sample_period in microsenconds for 1000.
80          */
81         I = pid_ctx->i_state * pid_ctx->cfg->ki * ((piddata_t)pid_ctx->cfg->sample_period / 1000);
82
83
84         /*
85          * Compute derivative contribute
86          */
87         D = (err - pid_ctx->prev_err) * pid_ctx->cfg->kd / ((piddata_t)pid_ctx->cfg->sample_period / 1000);
88
89
90         LOG_INFO("curr_pos[%lf],tgt[%lf],err[%f],P[%f],I[%f],D[%f]", curr_pos, target, err, P, I, D);
91
92
93         //Store the last error value
94         pid_ctx->prev_err = err;
95         piddata_t pid = MINMAX(pid_ctx->cfg->out_min, (P + I + D), pid_ctx->cfg->out_max);
96
97         LOG_INFO("pid[%lf]",pid);
98
99         //Clamp out between out_min and out_max
100         return pid;
101 }
102
103 /**
104  * Init PID control.
105  */
106 void pid_control_init(PidContext *pid_ctx, const PidCfg *pid_cfg)
107 {
108         /*
109          * Init all values of pid control struct
110          */
111         pid_ctx->cfg = pid_cfg;
112
113         pid_control_reset(pid_ctx);
114
115 }
116