Skip to content

Commit 0fd0948

Browse files
author
camilo
committed
small fix
1 parent 4c288a2 commit 0fd0948

File tree

2 files changed

+29
-29
lines changed

2 files changed

+29
-29
lines changed

‎src/include/pid.hpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -106,21 +106,21 @@ namespace qlibs {
106106
* @brief A PID controller object
107107
* @details The instance should be initialized using the pid::setup() method.
108108
*/
109-
class pidController : public pidGains, private nonCopyable {
109+
class pidController : public pidGains, public nState, private nonCopyable {
110110
private:
111111
//real_t Kc, Ki, Kd;
112112
real_t b, c, sat_Min, sat_Max, epsilon, kw, kt, D, u1, beta, uSat;
113113
real_t dt{ 1.0_re };
114114
real_t m, mInput;
115115
const real_t *yr{ nullptr };
116116
real_t alpha, gamma; /*MRAC additive controller parameters*/
117-
nState c_state; /*controller integral & derivative state*/
117+
//nState c_state; /*controller integral & derivative state*/
118118
nState m_state; /*MRAC additive controller state*/
119119
nState b_state; /*Bumpless-transfer state*/
120120
pidAutoTuning *adapt{ nullptr };
121121
pidMode mode{ pidMode::PID_AUTOMATIC };
122122
pidDirection dir{ pidDirection::PID_FORWARD };
123-
bool init{ false };
123+
bool isInitialized{ false };
124124
static real_t saturate( real_t x,
125125
const real_t vMin,
126126
const real_t vMax ) noexcept;

‎src/pid.cpp‎

Lines changed: 26 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ bool pidController::setup( const real_t kc,
1616

1717
if ( dt > 0.0_re ) {
1818
dt = dT;
19-
init = true;
19+
isInitialized = true;
2020
(void)setDerivativeFilter( 0.98_re );
2121
(void)setEpsilon( REAL_MIN );
2222
(void)setGains( kc, ki, kd );
@@ -36,7 +36,7 @@ bool pidController::setDirection( const pidDirection d ) noexcept
3636
{
3737
bool retValue = false;
3838

39-
if ( init ) {
39+
if ( isInitialized ) {
4040
dir = d;
4141
retValue = true;
4242
}
@@ -50,7 +50,7 @@ bool pidController::setParams( const real_t kc,
5050
{
5151
bool retValue = false;
5252

53-
if ( init ) {
53+
if ( isInitialized ) {
5454
Kc = kc;
5555
Ki = kc/ti;
5656
Kd = kc*td;
@@ -66,7 +66,7 @@ bool pidController::setGains( const real_t kc,
6666
{
6767
bool retValue = false;
6868

69-
if ( init ) {
69+
if ( isInitialized ) {
7070
Kc = kc;
7171
Ki = ki;
7272
Kd = kd;
@@ -80,7 +80,7 @@ bool pidController::setGains( const pidGains &g ) noexcept
8080
{
8181
bool retValue = false;
8282

83-
if ( init ) {
83+
if ( isInitialized ) {
8484
Kc = g.Kc;
8585
Ki = g.Ki;
8686
Kd = g.Kd;
@@ -95,7 +95,7 @@ bool pidController::setExtraGains( const real_t Kw,
9595
{
9696
bool retValue = false;
9797

98-
if ( init ) {
98+
if ( isInitialized ) {
9999
kw = Kw;
100100
kt = Kt;
101101
retValue = true;
@@ -109,7 +109,7 @@ bool pidController::setSaturation( const real_t Min,
109109
{
110110
bool retValue = false;
111111

112-
if ( init && ( Max > Min ) ) {
112+
if ( isInitialized && ( Max > Min ) ) {
113113
sat_Min = Min;
114114
sat_Max = Max;
115115
retValue = true;
@@ -122,7 +122,7 @@ bool pidController::setSeries( void ) noexcept
122122
{
123123
bool retValue = false;
124124

125-
if ( init ) {
125+
if ( isInitialized ) {
126126
real_t ti, td, tmp;
127127

128128
ti = Kc/Ki;
@@ -141,7 +141,7 @@ bool pidController::setEpsilon( const real_t eps ) noexcept
141141
{
142142
bool retValue = false;
143143

144-
if ( init ) {
144+
if ( isInitialized ) {
145145
epsilon = eps;
146146
retValue = true;
147147
}
@@ -153,7 +153,7 @@ bool pidController::setDerivativeFilter( const real_t Beta ) noexcept
153153
{
154154
bool retValue = false;
155155

156-
if ( init ) {
156+
if ( isInitialized ) {
157157
beta = Beta;
158158
retValue = true;
159159
}
@@ -165,7 +165,7 @@ bool pidController::setMode( const pidMode Mode ) noexcept
165165
{
166166
bool retValue = false;
167167

168-
if ( init ) {
168+
if ( isInitialized ) {
169169
mode = Mode;
170170
retValue = true;
171171
}
@@ -178,7 +178,7 @@ bool pidController::setReferenceWeighting( const real_t gb,
178178
{
179179
bool retValue = false;
180180

181-
if ( init ) {
181+
if ( isInitialized ) {
182182
b = saturate( gb, 0.0_re, 1.0_re );
183183
c = saturate( gc, 0.0_re, 1.0_re );
184184
retValue = true;
@@ -191,7 +191,7 @@ bool pidController::setManualInput( const real_t manualInput ) noexcept
191191
{
192192
bool retValue = false;
193193

194-
if ( init ) {
194+
if ( isInitialized ) {
195195
mInput = manualInput;
196196
retValue = true;
197197
}
@@ -203,8 +203,8 @@ bool pidController::reset( void ) noexcept
203203
{
204204
bool retValue = false;
205205

206-
if ( init ) {
207-
c_state.init();
206+
if ( isInitialized ) {
207+
init(); //internal controller state
208208
m_state.init();
209209
b_state.init();
210210
D = 0.0_re;
@@ -223,7 +223,7 @@ bool pidController::setModelReferenceControl( const real_t &modelRef,
223223
{
224224
bool retValue = false;
225225

226-
if ( init && ( Gamma > 0.0_re ) && ( Alpha > 0.0_re ) ) {
226+
if ( isInitialized && ( Gamma > 0.0_re ) && ( Alpha > 0.0_re ) ) {
227227
m_state.init();
228228
alpha = Alpha;
229229
gamma = Gamma;
@@ -238,7 +238,7 @@ bool pidController::removeModelReferenceControl( void ) noexcept
238238
{
239239
bool retValue = false;
240240

241-
if ( init ) {
241+
if ( isInitialized ) {
242242
yr = nullptr;
243243
retValue = true;
244244
}
@@ -251,7 +251,7 @@ real_t pidController::control( const real_t w,
251251
{
252252
real_t u = w;
253253

254-
if ( init ) {
254+
if ( isInitialized ) {
255255
real_t e, v, de, ie, bt, sw, kc, ki, kd;
256256
kc = Kc;
257257
ki = Ki;
@@ -265,8 +265,8 @@ real_t pidController::control( const real_t w,
265265
if ( ffmath::absf( e ) <= epsilon ) {
266266
e = 0.0_re;
267267
}
268-
de = c_state.derive( ( c*w ) - y , dt, false );
269-
ie = c_state.integrate( e + u1 , dt );
268+
de = derive( ( c*w ) - y , dt, false );
269+
ie = integrate( e + u1 , dt );
270270
D = de + beta*( D - de ); /*derivative filtering*/
271271
v = ( kc*( ( b*w ) - y ) ) + ( ki*ie ) + ( kd*D ); /*compute PID action*/
272272
if ( nullptr != yr ) {
@@ -346,22 +346,22 @@ bool pidAutoTuning::step( const real_t u,
346346
pidGains pidAutoTuning::getEstimates( void ) const noexcept
347347
{
348348
pidGains gains = { 0.0_re, 0.0_re, 0.0_re };
349-
const real_t td = ( tao/10.0_re );
349+
const real_t td = tao*0.1_re;
350350

351351
switch ( type ) {
352352
case pidType::PID_TYPE_P:
353-
gains.Kc = ( 1.03_re/k )*( ( tao/td ) + 0.34_re );
353+
gains.Kc = speed*( 1.03_re/k )*( ( tao/td ) + 0.34_re );
354354
break;
355355
case pidType::PID_TYPE_PD:
356-
gains.Kc = ( 1.24_re/k )*( ( tao/td ) + 0.129_re );
356+
gains.Kc = speed*( 1.24_re/k )*( ( tao/td ) + 0.129_re );
357357
gains.Kd = gains.Kc*( 0.27_re*td )*( tao - 0.324_re*td )/( tao + 0.129_re*td );
358358
break;
359359
case pidType::PID_TYPE_PI:
360-
gains.Kc = ( 0.9_re/k )*( ( tao/td ) + 0.092_re );
360+
gains.Kc = speed*( 0.9_re/k )*( ( tao/td ) + 0.092_re );
361361
gains.Ki = gains.Kc*( tao + 2.22_re*td )/( 3.33_re*td*( tao + 0.092_re*td ) );
362362
break;
363363
case pidType::PID_TYPE_PID:
364-
gains.Kc = ( 1.35_re/k )*( ( tao/td ) + 0.185_re );
364+
gains.Kc = speed*( 1.35_re/k )*( ( tao/td ) + 0.185_re );
365365
gains.Ki = gains.Kc*( tao + 0.611_re*td )/( 2.5_re*td*( tao + 0.185_re*td ) );
366366
gains.Kd = ( 0.37_re*gains.Kc*td*tao )/( tao + 0.185_re*td );
367367
break;
@@ -387,7 +387,7 @@ bool pidController::bindAutoTuning( pidAutoTuning &at ) noexcept
387387
{
388388
bool retValue = false;
389389

390-
if ( init ) {
390+
if ( isInitialized ) {
391391
real_t k, T;
392392

393393
adapt = &at;

0 commit comments

Comments
 (0)