fag/speciale/nodes/src/images/02V2_test_002/pidcontrol.c

   1  /*
   2  *  pidcontrol.c
   3  *
   4  *
   5  *  Created by Andreas Lyder on 31/03/08.
   6  *  Copyright 2008 __MyCompanyName__. All rights reserved.
   7  *
   8  */
   9
  10 /* INCLUDES */
  11 #include <malloc.h>
  12 #include <cyg/kernel/kapi.h>            /* All the kernel specific stuff */
  13 #include <cyg/hal/hal_arch.h>
  14 #include "pidcontrol.h"
  15
  16 /* DEFINES */
  17
  18 /* STATICS */
  19
  20 /* GLOBAL VARIABLES */
  21
  22 pidcontrol_t *create_pidcontrol(double pGain, double iGain, double dGain, double iMax, double iMin) {
  23   pidcontrol_t *pid = malloc(sizeof(pidcontrol_t));
  24
  25   pid->dState = 0;
  26   pid->iState = 0;
  27   pid->iMax = iMax;
  28   pid->iMin = iMin;
  29   pid->pGain = pGain;
  30   pid->iGain = iGain;
  31   pid->dGain = dGain;
  32
  33   return pid;
  34 }
  35
  36 void release_pidcontrol(pidcontrol_t *pid) {
  37   free(pid);
  38 }
  39
  40 double update_pidcontrol(pidcontrol_t * pid, double error, double position) {
  41   double pTerm,dTerm=0,iTerm=0;
  42
  43   pTerm = pid->pGain * error;
  44
  45   if (pid->iGain != 0) {
  46     pid->iState += error;
  47     if (pid->iState > pid->iMax) pid->iState = pid->iMax;
  48     else if (pid->iState < pid->iMin) pid->iState = pid->iMin;
  49     iTerm = pid->iGain * pid->iState;
  50   }
  51
  52   if (pid->dGain != 0) {
  53     dTerm = pid->dGain * (position - pid->dState);
  54     pid->dState = position;
  55   }
  56
  57   return pTerm + iTerm + dTerm;
  58 }
  59