mbsePi-apps: comparison brewco/pid.c

-:3ec477cda546
+:78e9d5234d15
 * 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"
-void InitPID(pid_var *pid)
-{
+/*
-pid->Err = pid->ErrLast = pid->iState = 0.0;
+* The parameters specified here are those for for which we can't set up
-pid->Input = pid->OutP = pid->SetP = 0.0;
+* reliable defaults, so we need to have the user set them.
-pid->pGain = pid->iGain = pid->dGain = 0.0;
+*/
-pid->Mode = PID_MODE_NONE;
+void PID_init(pid_var *pid, double *Input, double *Output, double *Setpoint, double Kp, double Ki, double Kd, int ControllerDirection)
-pid->iMax = 100.0;
+{
-pid->Times = PID_TIMES;
+pid->myOutput = Output;
-}
+pid->myInput = Input;
+pid->mySetpoint = Setpoint;
+pid->inAuto = FALSE;
+PID_setOutputLimits(pid, 0, 255);
-void UpdatePID(pid_var *pid)
+pid->SampleTime = 100;
-{
+PID_setControllerDirection(pid, ControllerDirection);
-if (pid->Mode == PID_MODE_AUTO) {
+PID_setTunings(pid, Kp, Ki, Kd);
+pid->lastTime = millis() - pid->SampleTime;
-	double	pTerm, dTerm, iTerm;
+}
-	pid->Err = pid->SetP - pid->Input;
-	/*
+/*
-	 * Calculate the integral state with appopriate limiting.
+* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
-	 * Use ErrLastLast as iState
+* when the transition from manual to auto occurs, the controller is
-	 */
+* automatically initialized
-	pid->iState += pid->Err;
+*/
-	if (pid->iState > PID_WINDUP_GUARD)
+void PID_setMode(pid_var *pid, int Mode)
-	    pid->iState = PID_WINDUP_GUARD;
+{
-	else if (pid->iState < -PID_WINDUP_GUARD)
+int		newAuto = (Mode == P_AUTOMATIC);
-	    pid->iState = -PID_WINDUP_GUARD;
+if (newAuto != pid->inAuto) {
-	pTerm = pid->pGain * pid->Err;
+	/*
-	iTerm = pid->iGain * pid->iState;
+	 * we just went from manual to auto
-	dTerm = pid->dGain * (pid->Err - pid->ErrLast);
+	 */
+	pid->ITerm = *pid->myOutput;
-	pid->OutP = pTerm + dTerm + iTerm;
+	pid->lastInput = *pid->myInput;
-	pid->ErrLast = pid->Err;
+	if (pid->ITerm > pid->outMax)
+	    pid->ITerm = pid->outMax;
-} else if (pid->Mode == PID_MODE_BOO) {
+	else if (pid->ITerm < pid->outMin)
-	/*
+	    pid->ITerm = pid->outMin;
-	 * Mode Bang On Off
+}
-	 */
+pid->inAuto = newAuto;
-	pid->ErrLast = pid->Err;
+}
-	pid->Err = pid->SetP - pid->Input;
-	if (pid->OutP && (pid->Err <= 0.0))
-	    pid->OutP = 0.0;
+/*
-	else if ((pid->OutP == 0.0) && (pid->Err > 0.0))
+* This function will be used far more often than SetInputLimits.  while
-	    pid->OutP = pid->iMax;
+* 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
-	pid->iState = 0.0;
+* 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
-} else {
+* here.
-	/*
+*/
-	 * While in manual mode, stay ready for bumpless switch to
+void PID_setOutputLimits(pid_var *pid, double Min, double Max)
-	 * auto.
+{
-	 */
+if (Min >= Max)
-	pid->ErrLast = pid->Err = 0.0;
+	return;
-	pid->OutP = pid->iState = 0.0;
-}
+pid->outMin = Min;
+pid->outMax = Max;
-if (pid->OutP > pid->iMax)
-	pid->OutP = pid->iMax;
+if (pid->inAuto == P_AUTOMATIC) {
-if (pid->OutP < 0.0)
+	if (*pid->myOutput > pid->outMax)
-	pid->OutP = 0.0;
+	    *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_setControllerDirection(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;
+}
+/*
+* 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;
+	/*
+	 * Remember some variables for the next time
+	 */
+	pid->lastInput = input;
+	pid->lastTime = now;
+	return TRUE;
+}
+return FALSE;
+}

Mercurial > mbsePi-apps / file comparison

comparison: brewco/pid.c

brewco/pid.c