rlcontroller.cpp File Reference

#include "rlcontroller.h"

Include dependency graph for rlcontroller.cpp:

Go to the source code of this file.

Functions
static void *	control (void *arg)

Function Documentation

control()

static void* control ( void *  arg )

static

Definition at line 18 of file rlcontroller.cpp.

{
  THREAD_PARAM *p = (THREAD_PARAM *) arg;
  rlController *c = (rlController *) p->user;
  while(c->running == 1)
  {
    if(c->sleepLocally) rlsleep(c->dt);
    c->measurement = c->getMeasurement();
    c->lock(); // lock mutex because user might set another controller type
    c->ydk_1 = c->ydk;
    c->y1k_1 = c->y1k;
    c->yk_2  = c->yk_1;
    c->yk_1  = c->yk;
    c->ek_2  = c->ek_1;
    c->ek_1  = c->ek;
    c->ek    = c->reference - c->measurement;
    switch(c->type)
    {
      case rlController::P:
        c->yk = c->d0*c->ek;
        break;
      case rlController::I:
      case rlController::D_T1:
      case rlController::PI:
      case rlController::PD_T1:
        c->yk = c->d0*c->ek + c->d1*c->ek_1 + c->c1*c->yk_1;
        break;
      case rlController::PID_T1:
        c->yk = c->d0*c->ek + c->d1*c->ek_1 + c->d2*c->ek_2 + c->c1*c->yk_1 + c->c2*c->yk_2;
        break;
      case rlController::PI_SUM:
        c->yk = c->Kp*c->ek + c->y1k;
        c->y1k = c->y1k_1 + c->d1*(c->ek+c->ek_1);
        break;
      case rlController::PD_T1_SUM:
        c->yk = c->Kp*c->ek + c->ydk;
        c->ydk = c->cD*c->ydk_1 + c->dD*(c->ek-c->ek_1);
        break;
      case rlController::PID_T1_SUM:
        c->yk = c->Kp*c->ek + c->y1k + c->ydk;
        c->y1k = c->y1k_1 + c->d1*(c->ek+c->ek_1);
        c->ydk = c->cD*c->ydk_1 + c->dD*(c->ek-c->ek_1);
        break;
      default:
        break;
    }
    if(c->limited)
    {
      if(c->yk  > c->yk_max) c->yk  = c->yk_max;
      if(c->yk  < c->yk_min) c->yk  = c->yk_min;
      if(c->y1k > c->yk_max) c->y1k = c->yk_max;
      if(c->y1k < c->yk_min) c->y1k = c->yk_min;
      if(c->ydk > c->yk_max) c->ydk = c->yk_max;
      if(c->ydk < c->yk_min) c->ydk = c->yk_min;
    }
    c->unlock();
    c->writeOutput(c->yk);
  }
  return arg;
}