brewco/pid.c

/*****************************************************************************
 * 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.
 *
 * Based on the Arduino PID Library 1.1.1 by Brett Beauregard <br3ttb@gmail.com>
 * This Library is licensed under a GPLv3 License
 *****************************************************************************/

#include "brewco.h"
#include "pid.h"
#include "util.h"


/*
 * The parameters specified here are those for for which we can't set up
 * reliable defaults, so we need to have the user set them.
 */
void PID_init(pid_var *pid, double *Input, double *Output, double *Setpoint, double Kp, double Ki, double Kd, int ControllerDirection)
{
    pid->myOutput = Output;
    pid->myInput = Input;
    pid->mySetpoint = Setpoint;
    pid->inAuto = FALSE;
    PID_setOutputLimits(pid, 0, 255);
    pid->SampleTime = 100;
    PID_setDirection(pid, ControllerDirection);
    PID_setTunings(pid, Kp, Ki, Kd);
    pid->lastTime = millis() - pid->SampleTime;
}


/*
 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
 * when the transition from manual to auto occurs, the controller is
 * automatically initialized
 */
void PID_setMode(pid_var *pid, int Mode)
{
    int		newAuto = (Mode == P_AUTOMATIC);

    if (newAuto != pid->inAuto) {
	/*
	 * we just went from manual to auto
	 */
	pid->ITerm = *pid->myOutput;
	pid->lastInput = *pid->myInput;
	if (pid->ITerm > pid->outMax)
	    pid->ITerm = pid->outMax;
	else if (pid->ITerm < pid->outMin)
	    pid->ITerm = pid->outMin;
	/*
	 * If turned to manual, turn output off.
	 */
	if (Mode == P_MANUAL)
	    *pid->myOutput = pid->outMin;
    }
    pid->inAuto = newAuto;
}


/*
 * This function will be used far more often than SetInputLimits.  while
 * the input to the controller will generally be in the 0-1023 range (which is
 * the default already,)  the output will be a little different.  maybe they'll
 * be doing a time window and will need 0-8000 or something.  or maybe they'll
 * want to clamp it from 0-125.  who knows.  at any rate, that can all be done
 * here.
 */
void PID_setOutputLimits(pid_var *pid, double Min, double Max)
{
    if (Min >= Max)
	return;

    pid->outMin = Min;
    pid->outMax = Max;

    if (pid->inAuto == P_AUTOMATIC) {
	if (*pid->myOutput > pid->outMax)
	    *pid->myOutput = pid->outMax;
	else if (*pid->myOutput < pid->outMin)
	    *pid->myOutput = pid->outMin;

	if (pid->ITerm > pid->outMax)
	    pid->ITerm = pid->outMax;
	else if (pid->ITerm < pid->outMin)
	    pid->ITerm = pid->outMin;
    }
}


/*
 * This function allows the controller's dynamic performance to be adjusted.
 * it's called automatically from the constructor, but tunings can also
 * be adjusted on the fly during normal operation.
 */
void PID_setTunings(pid_var *pid, double Kp, double Ki, double Kd)
{
    if (Kp < 0 || Ki < 0 || Kd < 0)
	return;
    pid->dispKp = Kp;
    pid->dispKi = Ki;
    pid->dispKd = Kd;

    double SampleTimeInSec = ((double)pid->SampleTime) / 1000;
    pid->Kp = Kp;
    pid->Ki = Ki * SampleTimeInSec;
    pid->Kd = Kd / SampleTimeInSec;

    if (pid->Direction == P_REVERSE) {
	pid->Kp = (0 - pid->Kp);
	pid->Ki = (0 - pid->Ki);
	pid->Kd = (0 - pid->Kd);
    }
}


/*
 * The PID will either be connected to a DIRECT acting process (+Output leads
 * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
 * know which one, because otherwise we may increase the output when we should
 * be decreasing.  This is called from PID_init().
 */
void PID_setDirection(pid_var *pid, int Direction)
{
    if (pid->inAuto && Direction != pid->Direction) {
	pid->Kp = (0 - pid->Kp);
	pid->Ki = (0 - pid->Ki);
	pid->Kd = (0 - pid->Kd);
    }
    pid->Direction = Direction;
}


/*
 * sets the period, in Milliseconds, at which the calculation is performed.
 */
void PID_setSampleTime(pid_var *pid, int NewSampleTime)
{
    if (NewSampleTime > 0) {
	double	ratio = (double)NewSampleTime / (double)pid->SampleTime;

	pid->Ki *= ratio;
	pid->Kd /= ratio;
	pid->SampleTime = NewSampleTime;
    }
}


/*
 * Just because you set the Kp=-1 doesn't mean it actually happened.  these
 * functions query the internal state of the PID.  they're here for display
 * purposes.  this are the functions the PID Front-end uses for example
 */
double PID_getKp(pid_var *pid)
{
    return pid->dispKp;
}


double PID_getKi(pid_var *pid)
{
    return pid->dispKi;
}


double PID_getKd(pid_var *pid)
{
    return pid->dispKd;
}


int PID_getMode(pid_var *pid)
{
    return pid->inAuto ? P_AUTOMATIC : P_MANUAL;
}


int PID_getDirection(pid_var *pid)
{
    return pid->Direction;
}


int PID_getSampleTime(pid_var *pid)
{
    return pid->SampleTime;
}


/*
 * This, as they say, is where the magic happens.  this function should be called
 * every time "void loop()" executes.  the function will decide for itself whether a new
 * pid Output needs to be computed.  returns true when the output is computed,
 * false when nothing has been done.
 */
int PID_compute(pid_var *pid)
{
    if (! pid->inAuto)
	return FALSE;

    long now = millis();
    long timeChange = (now - pid->lastTime);

    if (timeChange >= pid->SampleTime) {
	/*
	 * Compute all the working error variables
	 */
	double input = *pid->myInput;
	double error = *pid->mySetpoint - input;
	pid->ITerm += (pid->Ki * error);
	if (pid->ITerm > pid->outMax)
	    pid->ITerm = pid->outMax;
	else if (pid->ITerm < pid->outMin)
	    pid->ITerm = pid->outMin;
	double dInput = input - pid->lastInput;

	/*
	 * Compute PID output
	 */
	double output = pid->Kp * error + pid->ITerm - pid->Kd * dInput;
	if (output > pid->outMax)
	    output = pid->outMax;
	else if (output < pid->outMin)
	    output = pid->outMin;
	*pid->myOutput = output;

	/*
	 * Remember some variables for the next time
	 */
	pid->lastInput = input;
	pid->lastTime = now;
	return TRUE;
    }

    return FALSE;
}