Fri, 11 Dec 2015 15:52:37 +0100
Beginning of the main program loop
/***************************************************************************** * 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; } 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; }