rllib  1
rlcontroller.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  rlcontroller.cpp - description
3  -------------------
4  begin : Wed Jun 16 2004
5  copyright : (C) 2004 by R. Lehrig
6  email : lehrig@t-online.de
7  ***************************************************************************/
8 
9 /***************************************************************************
10  * *
11  * This library is free software; you can redistribute it and/or modify *
12  * it under the terms of the GNU LESSER GENERAL PUBLIC LICENSE as *
13  * published by the Free Software Foundation *
14  * *
15  ***************************************************************************/
16 #include "rlcontroller.h"
17 
18 static void *control(void *arg)
19 {
20  THREAD_PARAM *p = (THREAD_PARAM *) arg;
21  rlController *c = (rlController *) p->user;
22  while(c->running == 1)
23  {
24  if(c->sleepLocally) rlsleep(c->dt);
25  c->measurement = c->getMeasurement();
26  c->lock(); // lock mutex because user might set another controller type
27  c->ydk_1 = c->ydk;
28  c->y1k_1 = c->y1k;
29  c->yk_2 = c->yk_1;
30  c->yk_1 = c->yk;
31  c->ek_2 = c->ek_1;
32  c->ek_1 = c->ek;
33  c->ek = c->reference - c->measurement;
34  switch(c->type)
35  {
36  case rlController::P:
37  c->yk = c->d0*c->ek;
38  break;
39  case rlController::I:
40  case rlController::D_T1:
41  case rlController::PI:
42  case rlController::PD_T1:
43  c->yk = c->d0*c->ek + c->d1*c->ek_1 + c->c1*c->yk_1;
44  break;
45  case rlController::PID_T1:
46  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;
47  break;
48  case rlController::PI_SUM:
49  c->yk = c->Kp*c->ek + c->y1k;
50  c->y1k = c->y1k_1 + c->d1*(c->ek+c->ek_1);
51  break;
52  case rlController::PD_T1_SUM:
53  c->yk = c->Kp*c->ek + c->ydk;
54  c->ydk = c->cD*c->ydk_1 + c->dD*(c->ek-c->ek_1);
55  break;
56  case rlController::PID_T1_SUM:
57  c->yk = c->Kp*c->ek + c->y1k + c->ydk;
58  c->y1k = c->y1k_1 + c->d1*(c->ek+c->ek_1);
59  c->ydk = c->cD*c->ydk_1 + c->dD*(c->ek-c->ek_1);
60  break;
61  default:
62  break;
63  }
64  if(c->limited)
65  {
66  if(c->yk > c->yk_max) c->yk = c->yk_max;
67  if(c->yk < c->yk_min) c->yk = c->yk_min;
68  if(c->y1k > c->yk_max) c->y1k = c->yk_max;
69  if(c->y1k < c->yk_min) c->y1k = c->yk_min;
70  if(c->ydk > c->yk_max) c->ydk = c->yk_max;
71  if(c->ydk < c->yk_min) c->ydk = c->yk_min;
72  }
73  c->unlock();
74  c->writeOutput(c->yk);
75  }
76  return arg;
77 }
78 
79 rlController::rlController(double (*_getMeasurement)() ,void (_writeOutput)(double output))
80  :rlThread()
81 {
82  type = -1;
83  running = 0;
84  getMeasurement = _getMeasurement;
85  writeOutput = _writeOutput;
86  ydk_1 = 0.0f;
87  ydk = 0.0f;
88  y1k_1 = 0.0f;
89  y1k = 0.0f;
90  yk_2 = 0.0f;
91  yk_1 = 0.0f;
92  yk = 0.0f;
93  ek_2 = 0.0f;
94  ek_1 = 0.0f;
95  ek = 0.0f;
96  limited = 0;
97  yk_min = -999999;
98  yk_max = 999999;
99  reference = measurement = 0.0;
100  sleepLocally = 1;
101 }
102 
103 rlController::~rlController()
104 {
105  stop();
106 }
107 
108 void rlController::start()
109 {
110  running = 1;
111  ydk_1 = 0.0f;
112  ydk = 0.0f;
113  y1k_1 = 0.0f;
114  y1k = 0.0f;
115  yk_2 = 0.0f;
116  yk_1 = 0.0f;
117  yk = 0.0f;
118  ek_2 = 0.0f;
119  ek_1 = 0.0f;
120  ek = 0.0f;
121  rlThread::create(control,this);
122 }
123 
124 void rlController::stop()
125 {
126  if(running) rlThread::cancel();
127  running = 0;
128 }
129 
130 void rlController::setReference(double _reference)
131 {
132  reference = _reference;
133 }
134 
135 void rlController::setP(double _T, double _Kp)
136 {
137  rlThread::lock();
138  type = P;
139  T = _T;
140  Kp = _Kp;
141  dt = (int) (1000.0 * T);
142  d0 = Kp;
143  rlThread::unlock();
144 }
145 
146 void rlController::setI(double _T, double _T1)
147 {
148  rlThread::lock();
149  type = I;
150  T = _T;
151  T1 = _T1;
152  dt = (int) (1000.0 * T);
153  d0 = T/(2.0*T1);
154  d1 = T/(2.0*T1);
155  c1 = 1.0;
156  rlThread::unlock();
157 }
158 
159 void rlController::setD_T1(double _T, double _TD, double _Td)
160 {
161  rlThread::lock();
162  type = D_T1;
163  T = _T;
164  TD = _TD;
165  Td = _Td;
166  dt = (int) (1000.0 * T);
167  d0 = TD / (Td + T/2.0);
168  d1 = -TD / (Td + T/2.0);
169  c1 = (Td-T/2.0)/(Td+T/2.0);
170  rlThread::unlock();
171 }
172 
173 void rlController::setPI(double _T, double _Kp, double _Tn)
174 {
175  rlThread::lock();
176  type = PI;
177  T = _T;
178  Kp = _Kp;
179  Tn = _Tn;
180  dt = (int) (1000.0 * T);
181  d0 = Kp * (Tn+T/2.0) / Tn;
182  d1 = -Kp * (Tn - T/2.0) / Tn;
183  c1 = 1.0;
184  rlThread::unlock();
185 }
186 
187 void rlController::setPD_T1(double _T, double _Kp, double _TvP, double _Td)
188 {
189  rlThread::lock();
190  type = PD_T1;
191  T = _T;
192  Kp = _Kp;
193  TvP = _TvP;
194  Td = _Td;
195  dt = (int) (1000.0 * T);
196  d0 = Kp * (TvP+T/2.0) / (Td + T/2.0);
197  d1 = -Kp * (TvP - T/2.0) / (Td + T/2.0);
198  c1 = (Td - T/2.0)/(Td + T/2.0);
199  rlThread::unlock();
200 }
201 
202 void rlController::setPID_T1(double _T, double _Kpp, double _TnP, double _TvP, double _Td)
203 {
204  rlThread::lock();
205  type = PID_T1;
206  T = _T;
207  Kpp = _Kpp;
208  TnP = _TnP;
209  TvP = _TvP;
210  Td = _Td;
211  dt = (int) (1000.0 * T);
212  d0 = Kpp * ((TnP+T/2.0) / TnP) * ((TvP+T/2.0))/(Td+T/2.0);
213  d1 = -2.0*Kpp * (TnP*TvP - (T/2.0)*(T/2.0)) / (TnP*(Td+T/2.0));
214  d2 = Kpp*((TnP-T/2.0)/TnP)*((TvP-T/2.0)/(Td+T/2.0));
215  c1 = 2*Td/(Td+T/2.0);
216  c2 = -(Td-T/2.0)/(Td+T/2.0);
217  rlThread::unlock();
218 }
219 
220 void rlController::setPI_SUM(double _T, double _Kp, double _Tn)
221 {
222  rlThread::lock();
223  type = PI_SUM;
224  T = _T;
225  Kp = _Kp;
226  Tn = _Tn;
227  dt = (int) (1000.0 * T);
228  d1 = Kp * (T/2.0)/Tn;
229  rlThread::unlock();
230 }
231 
232 void rlController::setPD_T1_SUM(double _T, double _Kp, double _Tv, double _Td)
233 {
234  rlThread::lock();
235  type = PD_T1_SUM;
236  T = _T;
237  Kp = _Kp;
238  Tv = _Tv;
239  Td = _Td;
240  dt = (int) (1000.0 * T);
241  dD = Kp * (Tv/(Td+T/2.0));
242  cD = (Td-T/2.0)/(Td+T/2.0);
243  rlThread::unlock();
244 }
245 
246 void rlController::setPID_T1_SUM(double _T, double _Kp, double _Tn, double _Tv, double _Td)
247 {
248  rlThread::lock();
249  type = PID_T1_SUM;
250  T = _T;
251  Kp = _Kp;
252  Tn = _Tn;
253  Tv = _Tv;
254  Td = _Td;
255  dt = (int) (1000.0 * T);
256  d1 = Kp*T/(2.0*Tn);
257  dD = Kp * (Tv/(Td+T/2.0));
258  cD = (Td-T/2.0)/(Td+T/2.0);
259  rlThread::unlock();
260 }
261 
262 void rlController::setLimits(double _yk_min, double _yk_max)
263 {
264  rlThread::lock();
265  yk_min = _yk_min;
266  yk_max = _yk_max;
267  limited = 1;
268  rlThread::unlock();
269 }
270 
271 void rlController::resetLimits()
272 {
273  rlThread::lock();
274  yk_min = -999999;
275  yk_max = 999999;
276  limited = 0;
277  rlThread::unlock();
278 }
279 
rlController::setLimits
void setLimits(double _yk_min, double _yk_max)
Definition: rlcontroller.cpp:262
rlThread::cancel
int cancel()
Definition: rlthread.cpp:78
rlController::setI
void setI(double _T, double _T1)
Definition: rlcontroller.cpp:146
THREAD_PARAM::user
void * user
Definition: rlthread.h:30
rlController::writeOutput
void(* writeOutput)(double output)
Definition: rlcontroller.h:140
rlController::rlController
rlController(double(*_getMeasurement)(), void(_writeOutput)(double output))
Definition: rlcontroller.cpp:79
rlThread::unlock
int unlock()
Definition: rlthread.cpp:52
THREAD_PARAM
Definition: rlthread.h:27
rlController::setPID_T1
void setPID_T1(double _T, double _Kpp, double _TnP, double _TvP, double _Td)
Definition: rlcontroller.cpp:202
rlThread::create
int create(void *(*func)(void *), void *argument)
Definition: rlthread.cpp:35
rlController::setPI_SUM
void setPI_SUM(double _T, double _Kp, double _Tn)
Definition: rlcontroller.cpp:220
rlController::measurement
double measurement
Definition: rlcontroller.h:154
rlController::setPID_T1_SUM
void setPID_T1_SUM(double _T, double _Kp, double _Tn, double _Tv, double _Td)
Definition: rlcontroller.cpp:246
rlController::setP
void setP(double _T, double _Kp)
Definition: rlcontroller.cpp:135
rlController::setPI
void setPI(double _T, double _Kp, double _Tn)
Definition: rlcontroller.cpp:173
rlsleep
void rlsleep(long msec)
Definition: rlwthread.cpp:396
rlController::getMeasurement
double(* getMeasurement)()
Definition: rlcontroller.h:136
rlController::setPD_T1_SUM
void setPD_T1_SUM(double _T, double _Kp, double _Tv, double _Td)
Definition: rlcontroller.cpp:232
rlController::setReference
void setReference(double _reference)
Definition: rlcontroller.cpp:130
rlController::setD_T1
void setD_T1(double _T, double _TD, double _Td)
Definition: rlcontroller.cpp:159
rlThread::lock
int lock()
Definition: rlthread.cpp:47
rlController::reference
double reference
Definition: rlcontroller.h:123
rlController::setPD_T1
void setPD_T1(double _T, double _Kp, double _TvP, double _Td)
Definition: rlcontroller.cpp:187
control
static void * control(void *arg)
Definition: rlcontroller.cpp:18
rlController::resetLimits
void resetLimits()
Definition: rlcontroller.cpp:271
Generated by   doxygen 1.8.14