/** * \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) * * * \author Daniele Basile <asterix@develer.com> */ #include "pid_control.h" #include "cfg/cfg_pid.h" // Define logging setting (for cfg/log.h module). #define LOG_LEVEL PID_LOG_LEVEL #define LOG_VERBOSITY PID_LOG_FORMAT #include <cfg/log.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); LOG_INFO("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); LOG_INFO("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); }