Mercurial > mbsePi-apps / file revision

/*****************************************************************************
 * Copyright (C) 2015
 *   
 * Michiel Broek <mbroek at mbse dot eu>
 *
 * This file is part of the mbsePi-apps
 *
 * This 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, or (at your option) any
 * later version.
 *
 * mbsePi-apps 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 ThermFerm; see the file COPYING.  If not, write to the Free
 * Software Foundation, 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
 *****************************************************************************/

#include "thermferm.h"
#include "pid.h"


double UpdatePID(pid_var *pid, double error, double position)
{
    double	pTerm, dTerm, iTerm;

    pTerm = pid->pGain * error;

    /*
     * Calculate the integral state with appopriate limiting
     */
    pid->iState += error;
    if (pid->iState > pid->iMax)
	pid->iState = pid->iMax;
    else if (pid->iState < pid->iMin)
	pid->iState = pid->iMin;

    iTerm = pid->iGain * pid->iState;

    dTerm = pid->dGain * (pid->dState - position);
    pid->dState = position;

    return pTerm + dTerm + iTerm;
}