Add PID controll module.

git-svn-id: https://src.develer.com/svnoss/bertos/trunk@1234 38d2e660-2303-0410-9eaa-f027e97ec537

diff --git a/bertos/algo/pid_control.c b/bertos/algo/pid_control.c
new file mode 100644 (file)
index 0000000..90a368f
--- /dev/null
+++ b/bertos/algo/pid_control.c
@@ -0,0 +1,109 @@
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction.  Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License.  This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2008 Develer S.r.l. (http://www.develer.com/)
+ * -->
+ *
+ *
+ * \brief Proportional, integral, derivative controller (PID controller) (implementation)
+ *
+ * \version $Id$
+ *
+ * \author Daniele Basile <asterix@develer.com>
+ */
+
+#include "pid_control.h"
+
+#include <cfg/debug.h>
+
+/**
+ * Compute next value for reaching \a target point.
+ */
+piddata_t pid_control_update(PidContext *pid_ctx, piddata_t target, piddata_t curr_pos)
+{
+       piddata_t P;
+       piddata_t I;
+       piddata_t D;
+       piddata_t err;
+
+       //Compute current error.
+       err = target - curr_pos;
+
+       /*
+        * Compute Proportional contribute
+        */
+       P = err * pid_ctx->cfg->kp;
+
+       //Update integral state error
+       pid_ctx->i_state += err;
+
+       //Clamp integral state between i_min and i_max
+       pid_ctx->i_state  = MINMAX(pid_ctx->cfg->i_min, pid_ctx->i_state, pid_ctx->cfg->i_max);
+
+       /*
+        * Compute Integral contribute
+        *
+        * note: for computing the integral contribute we use a sample period in seconds
+        * and so we divide sample_period in microsenconds for 1000.
+        */
+       I = pid_ctx->i_state * pid_ctx->cfg->ki * ((piddata_t)pid_ctx->cfg->sample_period / 1000);
+
+
+       /*
+        * Compute derivative contribute
+        */
+       D = (err - pid_ctx->prev_err) * pid_ctx->cfg->kd / ((piddata_t)pid_ctx->cfg->sample_period / 1000);
+
+
+//     TRACEMSG("curr_pos[%lf],tgt[%lf],err[%f],P[%f],I[%f],D[%f]", curr_pos, target, err, P, I, D);
+
+
+       //Store the last error value
+       pid_ctx->prev_err = err;
+       piddata_t pid = MINMAX(pid_ctx->cfg->out_min, (P + I + D), pid_ctx->cfg->out_max);
+
+//     TRACEMSG("pid[%lf]",pid);
+
+       //Clamp out between out_min and out_max
+       return pid;
+}
+
+/**
+ * Init PID control.
+ */
+void pid_control_init(PidContext *pid_ctx, const PidCfg *pid_cfg)
+{
+       /*
+        * Init all values of pid control struct
+        */
+       pid_ctx->cfg = pid_cfg;
+
+       pid_control_reset(pid_ctx);
+
+}
+

diff --git a/bertos/algo/pid_control.h b/bertos/algo/pid_control.h
new file mode 100644 (file)
index 0000000..74cdcd1
--- /dev/null
+++ b/bertos/algo/pid_control.h
@@ -0,0 +1,113 @@
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction.  Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License.  This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2008 Develer S.r.l. (http://www.develer.com/)
+ * -->
+ *
+ *
+ * \brief Proportional, integral, derivative controller (PID controller) (interface)
+ *
+ * \version $Id$
+ *
+ * \author Daniele Basile <asterix@develer.com>
+ */
+
+#ifndef ALGO_PID_CONTROL_H
+#define ALGO_PID_CONTROL_H
+
+#include <drv/timer.h>
+
+/**
+ * Data type for pid coefficient.
+ */
+typedef float pidk_t;
+typedef float piddata_t;
+
+/**
+ * PID context structure.
+ */
+typedef struct PidCfg
+{
+       pidk_t kp;              ///< Proportional term of PID control method (Gain).
+       pidk_t ki;              ///< Integral term of PID control method (Integral Gain).
+       pidk_t kd;              ///< Derivative of PID control method (Derivative Gain).
+
+       piddata_t i_max;        ///< Max value of integral term.
+       piddata_t i_min;        ///< Min value of integral term.
+
+       piddata_t out_max;      ///< Man value of output.
+       piddata_t out_min;      ///< Min value of output.
+
+       mtime_t sample_period;  ///< Sample period in milliseconds.
+
+} PidCfg;
+
+
+/**
+ * PID context structure.
+ */
+typedef struct PidContext
+{
+       const PidCfg *cfg;
+
+       piddata_t prev_err;     ///< Previous error.
+       piddata_t i_state;      ///< Integrator state (sum of all the preceding errors).
+
+} PidContext;
+
+/**
+ * Set Kp, Ki, Kd constants of PID control.
+ */
+INLINE void pid_control_setPid(PidCfg *pid_cfg, pidk_t kp, pidk_t ki, pidk_t kd)
+{
+       pid_cfg->kp = kp;
+       pid_cfg->ki = ki;
+       pid_cfg->kd = kd;
+}
+
+/**
+ * Set sample period for PID control.
+ */
+INLINE void pid_control_setPeriod(PidCfg *pid_cfg, mtime_t sample_period)
+{
+       pid_cfg->sample_period = sample_period;
+}
+
+/**
+ * Clear a pid control structure
+ */
+INLINE void pid_control_reset(PidContext *pid_ctx)
+{
+       pid_ctx->i_state = 0;
+       pid_ctx->prev_err = 0;
+}
+
+piddata_t pid_control_update(PidContext *pid_ctx, piddata_t target, piddata_t curr_pos);
+void pid_control_init(PidContext *pid_ctx, const PidCfg *pid_cfg);
+
+#endif /* ALGO_PID_CONTROL_H */