rtpid.c

PID-REGULATOR FUNCTION "C" SOURCE CODE

/*****************************************************************/
/*							         */
/*   Copyright (c) Quinn-Curtis, 1991, 1992                      */
/*							         */
/*   Filename:  RTPID.C                                          */
/*   Revision:  3.0                                              */
/*   Date:      2/3/92                                         */
/*						                 */
/*   Description: -Routines for PID Control                      */
/*   Functions:                                                  */
/*   rtreseterrorterms   rtinitpidstat                           */
/*    rtfreepidstat       rtsetpidparameters  rtcalcpid          */
/*                                                               */
/*****************************************************************/

#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "rtstdhdr.h"
#include "rttcref.h"

#ifdef __cplusplus
extern "C" {
#endif


#define maxpidnum 128

typedef struct {  realtype olderror,      /* previous error value */
			   newerror,      /* new error value */
			   e1,e2,e3,      /* error term t-1, t-2 and t-3 */
			   sumerror,      /* sum of all previous errors */
			   steadystate,   /* steady state output position */
			   setpoint,      /* setpoint value */
			   sampleperiod,  /* time between adjacent updates */
			   kp,ki,kd,      /* proportional, integral and derivative constant values */
			   highclamp,lowclamp, /* high and low clamping value for output */
			   rateclamp,     /* first derivative of output should not exceed this rate */
			   mvfilter,      /* exponential smoothing constant */
			   lastmv,        /* previous smoothed value */
			   lastpidvalue;  /* last pid output value */
		  int      pidtype;       /* 0 = position pid, 1 = velocity  pid */
		  } rtpidrec;

typedef  rtpidrec *rtpidrecpntr;

rtpidrecpntr rtpidstat[maxpidnum];  /* array of pointers to rtpidrec */
int rtnumpid=0;  /* initial pid loop count */
/* reset all of the error term values in a pid loop */
void rtreseterrorterms(int loopnum) /* pid loop number */
{
   if (rtpidstat[loopnum] != NULL)  /* make sure pid loop initialized */
   {
     (*(rtpidstat[loopnum])).e1=0;       /* reset all error terms to 0 */
     (*(rtpidstat[loopnum])).e2=0;
     (*(rtpidstat[loopnum])).e3=0;
     (*(rtpidstat[loopnum])).olderror=0;
     (*(rtpidstat[loopnum])).newerror=0;
     (*(rtpidstat[loopnum])).sumerror=0;
  }
}
/* initialize the number of pid loops wanted */
void rtinitpidstat(int numpid)      /* number of pid loops to initialize */
{ int  i;
  for (i=0; i <=maxpidnum; i++)     /* make all pointer in array NULL */
    rtpidstat[i]=NULL;
  for (i=0; i <= numpid - 1; i++)   /* initialize numpid loops */
  {
    rtpidstat[i] = (rtpidrec *) calloc(1,sizeof(rtpidrec)); /* get rtpidstat buffer for each loop */
    rtreseterrorterms(i);           /* initialize error terms */
  }
  rtnumpid=numpid;
}

/* free memory associated with pid loops */
void rtfreepidstat(void)
{ int i;
  for (i=0; i <= rtnumpid - 1; i++)  /* loop through all initialized PID loops */
    if (rtpidstat[i] != NULL) free(rtpidstat[i]);
}

/* set parameters for a single pid loop */
void rtsetpidparameters(int loopnum,          /* pid loop number */
			int ptype,            /* pid type, 0 = position, 1 = velocity */
			realtype setpnt,      /* starting setpoint value */
			realtype steadstat,   /* steady state output value */
			realtype prop,        /* proportinal gain constant */
			realtype integ,       /* integral gain constant */
			realtype deriv,       /* derivative gain constant */
			realtype lowclmp,     /* low clamping value for output */
			realtype highclmp,    /* high clamping value for output */
			realtype rateclmp,    /* rate of change clamp on output */
			realtype sampleper,   /* update sample period */
			realtype filterconst) /* exponential smoothing filter value */
{
  if (rtpidstat[loopnum] != NULL)  /* make sure loop is valid */
   {  /* save all of the parameters */
     (*(rtpidstat[loopnum])).pidtype=ptype;
     (*(rtpidstat[loopnum])).setpoint=setpnt;
     (*(rtpidstat[loopnum])).steadystate=steadstat;
     (*(rtpidstat[loopnum])).highclamp= highclmp;
     (*(rtpidstat[loopnum])).lowclamp=lowclmp;
     (*(rtpidstat[loopnum])).rateclamp=rateclmp;
     (*(rtpidstat[loopnum])).sampleperiod=sampleper;
     (*(rtpidstat[loopnum])).lastpidvalue=steadstat;
     (*(rtpidstat[loopnum])).lastmv=0;
     (*(rtpidstat[loopnum])).mvfilter=filterconst;
     /* calculate PID constants based on sample interval */
     if (ptype==0) /* position algorithm */
     {
       (*(rtpidstat[loopnum])).kp=prop;
       (*(rtpidstat[loopnum])).ki=(prop * sampleper)*integ;
       (*(rtpidstat[loopnum])).kd=(prop *deriv)/sampleper;
     }
     else   /* velocity algorithm */
     {
       (*(rtpidstat[loopnum])).kp =
	    prop * (1.0 + sampleper * integ + deriv/sampleper);
       (*(rtpidstat[loopnum])).ki=-prop *(1.0 + 2.0 * (deriv/sampleper));
       (*(rtpidstat[loopnum])).kd=prop * deriv/sampleper;
     }
   }
}

/* calculate pid update, position algorithm */
realtype rtcalcpospid(int loopnum,          /* loop number */
		      realtype currentval,  /* new value for measured variable */
		      realtype setpnt)      /* new value for setpoint */
{ realtype  newsumerror,result, deltae, rateofchange, signofchange;
  result = 0.0;
  if (rtpidstat[loopnum] != NULL)  /* loop has been initialized with rtinitpidstat */
  {
      (*(rtpidstat[loopnum])).olderror=(*(rtpidstat[loopnum])).newerror;  /* old error becomes new error */
      (*(rtpidstat[loopnum])).e3=(*(rtpidstat[loopnum])).e2;  /* all errors shifted */
      (*(rtpidstat[loopnum])).e2=(*(rtpidstat[loopnum])).e1;
      (*(rtpidstat[loopnum])).e1=(*(rtpidstat[loopnum])).olderror;
      newsumerror=(*(rtpidstat[loopnum])).sumerror +
		     (*(rtpidstat[loopnum])).olderror;  /* error term summation */
      (*(rtpidstat[loopnum])).setpoint=setpnt;  /* save new setpoint */
      currentval=(1.0-(*(rtpidstat[loopnum])).mvfilter)*
		    currentval + (*(rtpidstat[loopnum])).mvfilter *
		    (*(rtpidstat[loopnum])).lastmv;  /* use exponential smoothing on current value */
      (*(rtpidstat[loopnum])).newerror=setpnt - currentval;  /* new error */

      deltae=((*(rtpidstat[loopnum])).newerror + 3.0 *
		((*(rtpidstat[loopnum])).e1-(*(rtpidstat[loopnum])).e2)
		- (*(rtpidstat[loopnum])).e3)/6.0;  /* error term used for derivative calculation */

      result= (*(rtpidstat[loopnum])).kp * (*(rtpidstat[loopnum])).newerror +
		 (*(rtpidstat[loopnum])).ki * newsumerror +
		 (*(rtpidstat[loopnum])).kd * deltae +
		 (*(rtpidstat[loopnum])).steadystate;  /* actual PID output calculation */

     rateofchange =(result-(*(rtpidstat[loopnum])).lastpidvalue)
		    /(*(rtpidstat[loopnum])).sampleperiod;  /* calculate rate of change of output */

     if (rateofchange >= 0.0) signofchange=1.0;
     else signofchange=-1.0;  /* take into account negative rate of change */
     if (fabs(rateofchange) > (*(rtpidstat[loopnum])).rateclamp)
	result=(*(rtpidstat[loopnum])).lastpidvalue +
		  signofchange * (*(rtpidstat[loopnum])).rateclamp *
		  (*(rtpidstat[loopnum])).sampleperiod;  /* if rate clamp, then modify output value */

     if (result > (*(rtpidstat[loopnum])).highclamp)   /* if high clamp, clamp to high clamp value */
       result=(*(rtpidstat[loopnum])).highclamp;
     else if (result < (*(rtpidstat[loopnum])).lowclamp)  /* if low clamp, clamp to low clamp value */
       result=(*(rtpidstat[loopnum])).lowclamp;
     else (*(rtpidstat[loopnum])).sumerror=newsumerror;  /* only sum error term if not in high or low clamp */


     (*(rtpidstat[loopnum])).lastmv=currentval;    /* save for use in exponential smoothing */
     (*(rtpidstat[loopnum])).lastpidvalue=result;  /* save pid value for use next calculation */

  }
 return(result);  /* return pid output value */
}

/* calculate pid update, velocity algorithm */
realtype rtcalcvelpid(int loopnum,             /* pid loop number */
		      realtype currentval,     /* new value for measured variable */
		      realtype setpnt)         /* new value for pid setpoint */
{ realtype result, rateofchange, signofchange;
  result = 0.0;
  if (rtpidstat[loopnum] != NULL)  /* loop has been initialized with rtinitpidstat */
  {
      (*(rtpidstat[loopnum])).e2=(*(rtpidstat[loopnum])).e1;        /* shift error terms one position */
      (*(rtpidstat[loopnum])).e1=(*(rtpidstat[loopnum])).newerror;

      currentval=(1.0-(*(rtpidstat[loopnum])).mvfilter)*
		    currentval + (*(rtpidstat[loopnum])).mvfilter *
		    (*(rtpidstat[loopnum])).lastmv;  /* use exponential smoothing on current value */

      (*(rtpidstat[loopnum])).setpoint=setpnt;    /* save new setpoint */
      (*(rtpidstat[loopnum])).newerror=setpnt - currentval;  /* new error term */

      result= (*(rtpidstat[loopnum])).kp * (*(rtpidstat[loopnum])).newerror +
		 (*(rtpidstat[loopnum])).ki * (*(rtpidstat[loopnum])).e1 +
		 (*(rtpidstat[loopnum])).kd * (*(rtpidstat[loopnum])).e2;   /* actual PID output calculation */

     rateofchange=result/(*(rtpidstat[loopnum])).sampleperiod;  /* calculate rate of change for output */
     if (rateofchange >= 0.0) signofchange=1.0;
     else signofchange=-1.0;  /* take into account negative rates */
     if (fabs(rateofchange) > (*(rtpidstat[loopnum])).rateclamp)
	result=signofchange * (*(rtpidstat[loopnum])).rateclamp *
		  (*(rtpidstat[loopnum])).sampleperiod;  /* if rate clamp, modify output value */

     if (result > (*(rtpidstat[loopnum])).highclamp)
       result=(*(rtpidstat[loopnum])).highclamp;    /* if high clamp then clamp to high clamp value */
     else if (result < (*(rtpidstat[loopnum])).lowclamp)
       result=(*(rtpidstat[loopnum])).lowclamp;     /* if low clamp then clamp to low clamp value */

     (*(rtpidstat[loopnum])).lastmv=currentval;     /* save for use in next exponential smooth calculation */
     (*(rtpidstat[loopnum])).lastpidvalue=result;   /* save pid value for use in next calculation */
  }
 return(result);   /* return pid output value */
}

/* general pid loop update */
realtype rtcalcpid(int loopnum,         /* pid loop number */
		   realtype currentval, /* current value for the measured variable */
		   realtype setpnt)     /* current value for the setpoint */
{ realtype result;
  if (rtpidstat[loopnum] != NULL)   /* check if valid loop number */
  {
    if ((*(rtpidstat[loopnum])).pidtype==0)   /* position algorithm */
      result=rtcalcpospid(loopnum,currentval,setpnt);
    else   /* velocity algorithm */
      result=rtcalcvelpid(loopnum,currentval,setpnt);
  }
  return(result);  /* return pid output calculation */
}



#ifdef __cplusplus
}
#endif