13 #include <yarp/os/Log.h> 14 #include <yarp/math/Math.h> 18 #define PID_SAT(x,L,H) ((x)>(H)?(H):((x)<(L)?(L):(x))) 28 Integrator::Integrator(
const double _Ts,
const Vector &y0,
const Matrix &_lim)
30 yAssert(_lim.rows()==y0.length());
31 dim=(
unsigned int)y0.length();
32 x_old.resize(dim,0.0);
42 Integrator::Integrator(
const double _Ts,
const Vector &y0)
44 dim=(
unsigned int)y0.length();
45 x_old.resize(dim,0.0);
73 for (
unsigned int i=0; i<dim; i++)
76 else if (res[i]>lim(i,1))
87 void Integrator::setSaturation(
bool _applySat)
96 void Integrator::setTs(
const double _Ts)
104 void Integrator::setLim(
const Matrix &_lim)
106 yAssert(_lim.rows()==dim);
115 const Vector& Integrator::integrate(
const Vector &
x)
117 yAssert(x.length()==dim);
128 void Integrator::reset(
const Vector &y0)
130 yAssert(y0.length()==dim);
137 void helperPID::addVectorToOption(Bottle &option,
const char *key,
const Vector &val)
139 Bottle &bKey=option.addList();
141 Bottle &bKeyContent=bKey.addList();
142 for (
size_t i=0; i<val.length(); i++)
143 bKeyContent.addDouble(val[i]);
148 bool helperPID::getVectorFromOption(
const Bottle &options,
const char *key,
149 Vector &val,
int &size)
151 if (options.check(key))
153 if (Bottle *b=options.find(key).asList())
155 int len=(int)val.length();
156 int bSize=(int)b->size();
158 size=bSize<len?bSize:len;
159 for (
int i=0; i<size; i++)
160 val[i]=b->get(i).asDouble();
171 parallelPID::parallelPID(
const double _Ts,
172 const Vector &_Kp,
const Vector &_Ki,
const Vector &_Kd,
173 const Vector &_Wp,
const Vector &_Wi,
const Vector &_Wd,
174 const Vector &_N,
const Vector &_Tt,
const Matrix &_satLim) :
175 Ts(_Ts), Kp(_Kp), Ki(_Ki), Kd(_Kd), Wp(_Wp), Wi(_Wi), Wd(_Wd),
176 N(_N), Tt(_Tt), satLim(_satLim)
178 dim=(
unsigned int)N.length();
181 uSat.resize(dim,0.0);
182 for (
unsigned int i=0; i<dim; i++)
183 uSat[i]=
PID_SAT(u[i],satLim(i,0),satLim(i,1));
188 Vector num(2),den(2);
189 for (
unsigned int i=0; i<dim; i++)
191 if ((Kp[i]!=0.0) && (N[i]!=0.0))
193 double tau=Kd[i]/(Kp[i]*N[i]);
194 num[0]=2.0; num[1]=-2.0;
195 den[0]=Ts+2.0*tau; den[1]=Ts-2.0*tau;
199 num[0]=num[1]=den[1]=0.0;
203 Der.push_back(
new Filter(num,den,u0));
213 const Vector& parallelPID::compute(
const Vector &ref,
const Vector &fb)
219 I=Int->integrate(Ki*(Wi*ref-fb)+(uSat-u)/Tt);
222 for (
unsigned int i=0; i<dim; i++)
227 Vector inputD=Kd*(Wd*ref-fb);
228 for (
unsigned int i=0; i<dim; i++)
230 Vector inputDi(1,inputD[i]);
231 Vector outputDi=Der[i]->filt(inputDi);
239 for (
unsigned int i=0; i<dim; i++)
240 uSat[i]=
PID_SAT(u[i],satLim(i,0),satLim(i,1));
247 void parallelPID::reset(
const Vector &u0)
249 size_t len=u0.length()>(size_t)dim?(
size_t)dim:u0.length();
253 for (
size_t i=0; i<len; i++)
264 void parallelPID::getOptions(Bottle &options)
266 Vector satLimVect(satLim.rows()*satLim.cols());
267 for (
int r=0; r<satLim.rows(); r++)
268 for (
int c=0; c<satLim.cols(); c++)
269 satLimVect[r*satLim.cols()+c]=satLim(r,c);
272 addVectorToOption(options,
"Kp",Kp);
273 addVectorToOption(options,
"Ki",Ki);
274 addVectorToOption(options,
"Kd",Kd);
275 addVectorToOption(options,
"Wp",Wp);
276 addVectorToOption(options,
"Wi",Wi);
277 addVectorToOption(options,
"Wd",Wd);
278 addVectorToOption(options,
"N",N);
279 addVectorToOption(options,
"Tt",Tt);
280 addVectorToOption(options,
"satLim",satLimVect);
282 Bottle &bTs=options.addList();
289 void parallelPID::setOptions(
const Bottle &options)
291 Vector satLimVect(satLim.rows()*satLim.cols());
292 for (
int r=0; r<satLim.rows(); r++)
293 for (
int c=0; c<satLim.cols(); c++)
294 satLimVect[r*satLim.cols()+c]=satLim(r,c);
296 bool recomputeQuantities=
false;
298 getVectorFromOption(options,
"Ki",Ki,size);
299 getVectorFromOption(options,
"Wp",Wp,size);
300 getVectorFromOption(options,
"Wi",Wi,size);
301 getVectorFromOption(options,
"Wd",Wd,size);
302 getVectorFromOption(options,
"Tt",Tt,size);
304 if (getVectorFromOption(options,
"Kp",Kp,size))
305 recomputeQuantities=
true;
307 if (getVectorFromOption(options,
"Kd",Kd,size))
308 recomputeQuantities=
true;
310 if (getVectorFromOption(options,
"N",N,size))
311 recomputeQuantities=
true;
313 if (getVectorFromOption(options,
"satLim",satLimVect,size))
315 for (
int r=0; r<satLim.rows(); r++)
316 for (
int c=0; c<satLim.cols(); c++)
317 satLim(r,c)=satLimVect[r*satLim.cols()+c];
319 recomputeQuantities=
true;
322 if (options.check(
"Ts"))
324 double _Ts=options.find(
"Ts").asDouble();
328 recomputeQuantities=
true;
332 if (recomputeQuantities)
334 for (
unsigned int i=0; i<dim; i++)
335 uSat[i]=
PID_SAT(uSat[i],satLim(i,0),satLim(i,1));
338 Vector num(2),den(2);
339 for (
unsigned int i=0; i<dim; i++)
341 if ((Kp[i]!=0.0) && (N[i]!=0.0))
343 double tau=Kd[i]/(Kp[i]*N[i]);
344 num[0]=2.0; num[1]=-2.0;
345 den[0]=Ts+2.0*tau; den[1]=Ts-2.0*tau;
349 num[0]=num[1]=den[1]=0.0;
353 Der[i]->adjustCoeffs(num,den);
361 if (getVectorFromOption(options,
"reset",v,size))
364 for (
int i=0; i<size; i++)
373 parallelPID::~parallelPID()
377 for (
unsigned int i=0; i<dim; i++)
385 seriesPID::seriesPID(
const double _Ts,
386 const Vector &_Kp,
const Vector &_Ti,
const Vector &_Kd,
387 const Vector &_N,
const Matrix &_satLim) :
388 Ts(_Ts), Kp(_Kp), Ti(_Ti), Kd(_Kd), N(_N), satLim(_satLim)
390 dim=(
unsigned int)N.length();
393 uSat.resize(dim,0.0);
394 for (
unsigned int i=0; i<dim; i++)
395 uSat[i]=
PID_SAT(u[i],satLim(i,0),satLim(i,1));
398 Vector num(2),den(2);
399 for (
unsigned int i=0; i<dim; i++)
401 num[0]=Ts; num[1]=Ts;
402 den[0]=Ts+2.0*Ti[i]; den[1]=Ts-2.0*Ti[i];
403 Int.push_back(
new Filter(num,den,u0));
405 if ((Kp[i]!=0.0) && (N[i]!=0.0))
407 double tau=Kd[i]/(Kp[i]*N[i]);
408 num[0]=2.0; num[1]=-2.0;
409 den[0]=Ts+2.0*tau; den[1]=Ts-2.0*tau;
413 num[0]=num[1]=den[1]=0.0;
417 Der.push_back(
new Filter(num,den,u0));
428 const Vector& seriesPID::compute(
const Vector &ref,
const Vector &fb)
434 for (
unsigned int i=0; i<dim; i++)
436 Vector inputDi(1,Kd[i]*
e[i]);
437 Vector outputDi=Der[i]->filt(inputDi);
445 for (
unsigned int i=0; i<dim; i++)
451 outputIi=Int[i]->filt(inputIi);
460 for (
unsigned int i=0; i<dim; i++)
461 uSat[i]=
PID_SAT(u[i],satLim(i,0),satLim(i,1));
468 void seriesPID::reset()
471 for (
unsigned int i=0; i<dim; i++)
480 void seriesPID::getOptions(Bottle &options)
482 Vector satLimVect(satLim.rows()*satLim.cols());
483 for (
int r=0; r<satLim.rows(); r++)
484 for (
int c=0; c<satLim.cols(); c++)
485 satLimVect[r*satLim.cols()+c]=satLim(r,c);
488 addVectorToOption(options,
"Kp",Kp);
489 addVectorToOption(options,
"Ti",Ti);
490 addVectorToOption(options,
"Kd",Kd);
491 addVectorToOption(options,
"N",N);
492 addVectorToOption(options,
"satLim",satLimVect);
494 Bottle &bTs=options.addList();
501 void seriesPID::setOptions(
const Bottle &options)
503 Vector satLimVect(satLim.rows()*satLim.cols());
504 for (
int r=0; r<satLim.rows(); r++)
505 for (
int c=0; c<satLim.cols(); c++)
506 satLimVect[r*satLim.cols()+c]=satLim(r,c);
508 bool recomputeQuantities=
false;
510 if (getVectorFromOption(options,
"Kp",Kp,size))
511 recomputeQuantities=
true;
513 if (getVectorFromOption(options,
"Ti",Ti,size))
514 recomputeQuantities=
true;
516 if (getVectorFromOption(options,
"Kd",Kd,size))
517 recomputeQuantities=
true;
519 if (getVectorFromOption(options,
"N",N,size))
520 recomputeQuantities=
true;
522 if (getVectorFromOption(options,
"satLim",satLimVect,size))
524 for (
int r=0; r<satLim.rows(); r++)
525 for (
int c=0; c<satLim.cols(); c++)
526 satLimVect[r*satLim.cols()+c]=satLim(r,c);
528 recomputeQuantities=
true;
531 if (options.check(
"Ts"))
533 double _Ts=options.find(
"Ts").asDouble();
537 recomputeQuantities=
true;
541 if (recomputeQuantities)
543 for (
unsigned int i=0; i<dim; i++)
544 uSat[i]=
PID_SAT(uSat[i],satLim(i,0),satLim(i,1));
547 Vector num(2),den(2);
548 for (
unsigned int i=0; i<dim; i++)
550 num[0]=Ts; num[1]=Ts;
551 den[0]=Ts+2.0*Ti[i]; den[1]=Ts-2.0*Ti[i];
552 Int[i]->adjustCoeffs(num,den);
555 if ((Kp[i]!=0.0) && (N[i]!=0.0))
557 double tau=Kd[i]/(Kp[i]*N[i]);
558 num[0]=2.0; num[1]=-2.0;
559 den[0]=Ts+2.0*tau; den[1]=Ts-2.0*tau;
563 num[0]=num[1]=den[1]=0.0;
567 Der[i]->adjustCoeffs(num,den);
572 if (options.check(
"reset"))
578 seriesPID::~seriesPID()
580 for (
unsigned int i=0; i<dim; i++)
Q15 saturate(const Q15result x, bool &saturated)
A class for defining a saturated integrator based on Tustin formula: .