Skip to content

Commit cab2ddf

Browse files
author
camilo
committed
pid refactor, added transportDelay to ltisys
1 parent 9d4b4ae commit cab2ddf

File tree

11 files changed

+231
-77
lines changed

11 files changed

+231
-77
lines changed

‎check/qlibs_cpp_test.cpp

Lines changed: 33 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -182,6 +182,7 @@ void test_fis( void )
182182
};
183183
real_t rulesStrength[ 3 ];
184184

185+
185186
tipper.setup( fis::Mamdani, tipper_inputs, tipper_outputs, MFin, MFout, rules, rulesStrength );
186187
tipper.setupInput( service, 0.0f, 1.0f );
187188
tipper.setupInput( food, 0.0f, 10.0f );
@@ -259,15 +260,22 @@ void test_crc( void )
259260

260261
void test_ltisys( void )
261262
{
263+
constexpr real_t dt = 0.05f;
262264
cout << "LTISYS TEST"<< endl;
263265
cout << "continuousSystem"<< endl;
264266
continuousTF<3> ctf= {
265267
{ 0.0f, 2.0f, 3.0f, 6.0f },
266268
{ 1.0f, 6.0f, 11.0f, 16.0f },
267269
};
268-
continuousSystem gc( ctf, 0.01f );
269-
for ( int i = 0; i < 1000; i++ ) {
270-
cout << gc.excite( 1.0f ) << endl;
270+
transportDelay<delayFromTime(1.3f, dt )> delay;
271+
272+
273+
continuousSystem gc( ctf, dt );
274+
275+
real_t t = 0.0;
276+
for ( int i = 0; i < 100; i++ ) {
277+
cout << t << " "<< delay( gc.excite( 1.0f ) ) << endl;
278+
t += dt;
271279
}
272280

273281
cout << "discreteSystem"<< endl;
@@ -279,8 +287,10 @@ void test_ltisys( void )
279287
real_t den[] = { 1.0f, -0.85f, 0.02f };
280288
discreteStates<3> xd= { 0.0f, 0.0f, 0.0f };
281289
discreteSystem gd( num, den, xd );
290+
282291
for ( int i = 0; i < 20; i++ ) {
283-
cout << gd.excite( 1.0f ) << endl;
292+
cout << gd.excite( 1.0f ) << endl;
293+
284294
}
285295

286296
}
@@ -526,8 +536,6 @@ bool operator<(const thing& lhs, const thing& rhs) {
526536

527537
int main()
528538
{
529-
530-
531539
thing things[] = {
532540
{1,0},
533541
{-2,5},
@@ -557,11 +565,26 @@ int main()
557565
for ( auto i : things ) {
558566
std::cout << i.a << " , "<< i.b << std::endl;
559567
}
568+
std::cout << "fill "<< std::endl;
569+
algorithm::fill( things, {0 ,0}, 1,2 );
570+
for ( auto i : things ) {
571+
std::cout << i.a << " , "<< i.b << std::endl;
572+
}
560573

561-
return 0;
574+
std::cout << "foreach "<< std::endl;
575+
algorithm::for_each( things, +[]( thing& t) { t.a++; } );
576+
577+
for ( auto i : things ) {
578+
std::cout << i.a << " , "<< i.b << std::endl;
579+
}
580+
581+
algorithm::any_of( things, +[](const thing x ) -> bool {
582+
return x.a < 3;
583+
} );
584+
585+
//return 0;
562586
test_crc();
563587
test_fp16();
564-
test_ltisys();
565588
test_tdl();
566589

567590
test_mat();
@@ -571,5 +594,7 @@ int main()
571594
test_fis3();
572595
test_ffmath();
573596

597+
test_ltisys();
598+
574599
return 0;
575600
}

‎check/settings/sa_check.dnx

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,15 @@
2020
<LogFile>_ "$PROJ_DIR$\LogFile1.log"</LogFile>
2121
<Category2>_ 2</Category2>
2222
</LogFile>
23+
<TerminalIO>
24+
<InputSource>1</InputSource>
25+
<InputMode2>10</InputMode2>
26+
<Filename>$PROJ_DIR$\TermIOInput.txt</Filename>
27+
<InputEcho>1</InputEcho>
28+
<ShowReset>0</ShowReset>
29+
<InputEncodingICU>0</InputEncodingICU>
30+
<OutputEncodingICU>0</OutputEncodingICU>
31+
</TerminalIO>
2332
<InterruptLog>
2433
<GraphEnabled>0</GraphEnabled>
2534
<LogEnabled>0</LogEnabled>
@@ -35,15 +44,6 @@
3544
<SumEnabled>0</SumEnabled>
3645
<ShowTimeSum>1</ShowTimeSum>
3746
</DataLog>
38-
<TerminalIO>
39-
<InputSource>1</InputSource>
40-
<InputMode2>10</InputMode2>
41-
<Filename>$PROJ_DIR$\TermIOInput.txt</Filename>
42-
<InputEcho>1</InputEcho>
43-
<ShowReset>0</ShowReset>
44-
<InputEncodingICU>0</InputEncodingICU>
45-
<OutputEncodingICU>0</OutputEncodingICU>
46-
</TerminalIO>
4747
<TermIOLog>
4848
<LoggingEnabled>_ 0</LoggingEnabled>
4949
<LogFile>_ ""</LogFile>

‎doc/qpid.dox

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@
5757
* As shown above, the derivative term \f$f_{d}(t)\f$ is the output of a
5858
* low-pass filter that takes the raw derivative as input.
5959
*
60-
* <center> \f$ f_{d}(t) = \text{LPF}[ \frac{de_{c}(t)}{dt} ]\f$ </center>
60+
* <center> \f$ f_{d}(t) = \text{LPF}[ \frac{de_{c}(t)}{dt} ] = de_c(t) + e^{-dt/T_f} \left [ f_d(t-1) - de_c(t)\right ]\f$ </center>
6161
*
6262
* and \f$ c(t)\f$, the saturation feedback for the anti-windup, with \f$K_{w}\f$
6363
* as the adjustment parameter.
@@ -140,7 +140,7 @@
140140
* real_t processMeasurement;
141141
* real_t controlOutput;
142142
* for ( ;; ) {
143-
* processMeasurement = BSP_ScaletoSpeed ( BSP_AnalogRead( BSP_AI_SPEED_CHANNEL ) );
143+
* processMeasurement = BSP_ScaletoSpeed( BSP_AnalogRead( BSP_AI_SPEED_CHANNEL ) );
144144
* controlOutput = controller->control( SetPoint, processMeasurement );
145145
* BSP_PWMSet( BSP_AO_SPEED_PWM_CHANNEL, BSP_ScaletoPWM( controlOutput ) );
146146
* vTaskDelay( dt / portTICK_RATE_MS) ;

‎keywords.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,8 @@ continuousStates KEYWORD1
3737
discreteStates KEYWORD1
3838
continuousTF KEYWORD1
3939
discreteTF KEYWORD1
40+
transportDelay KEYWORD1
41+
discreteDelay KEYWORD1
4042
ltisys KEYWORD1
4143
discreteSystem KEYWORD1
4244
continuousSystem KEYWORD1

‎library.json

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
"maintainer": true
1717
}
1818
],
19-
"version": "1.2.7",
19+
"version": "1.2.8",
2020
"license": "MIT",
2121
"frameworks": "arduino",
2222
"platforms": "*"

‎library.properties

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
name=qlibs
2-
version=1.2.7
2+
version=1.2.8
33
license=MIT
44
author=J. Camilo Gomez C. <[email protected]>
55
maintainer=J. Camilo Gomez C. <[email protected]>

‎src/CMakeLists.txt

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
cmake_minimum_required( VERSION 3.2 )
22
project( qlibs-cpp
3-
VERSION 1.2.7
3+
VERSION 1.2.8
44
DESCRIPTION "A collection of useful C++ libraries for embedded systems"
55
LANGUAGES CXX )
66

‎src/include/ltisys.hpp

Lines changed: 73 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,13 +55,22 @@ namespace qlibs {
5555
/**
5656
* @brief Continuous transfer function for easy LTI system definition
5757
* @note Initial conditions are zero by default.
58+
* @tparam order The continuous system order
5859
*/
5960
template<size_t order>
6061
struct continuousTF {
6162
real_t num[ order+1 ];
6263
real_t den[ order+1 ];
6364
continuousStates<order> states = {};
64-
continuousTF( const real_t ( &numerator )[ order + 1 ], const real_t ( &denominator )[ order + 1 ] ) {
65+
/**
66+
* @brief Constructor for the continuousTF class
67+
* @param[in] numerator An array of size <order> with the coefficients
68+
* of the numerator in descending powers of s.
69+
* @param[in] denominator An array of size <order> with the coefficients
70+
* of the denominator in descending powers of s.
71+
*/
72+
continuousTF( const real_t ( &numerator )[ order + 1 ], const real_t ( &denominator )[ order + 1 ] )
73+
{
6574
static_assert( order >= 1 , "Order should be greater than 0" );
6675
for ( size_t i = 0; i <= order; ++i ) {
6776
num[ i ] = numerator[ i ];
@@ -70,15 +79,78 @@ namespace qlibs {
7079
}
7180
};
7281

82+
83+
constexpr size_t delayFromTime( const real_t Time, const real_t dt )
84+
{
85+
return static_cast<size_t>( ( Time/dt ) + 0.5_re );
86+
}
87+
88+
/**
89+
* @brief Delays the input by a specified amount of time. You can use this
90+
* class to simulate a time delay.
91+
* @tparam numberOfDelay The number of discrete delays to be used. Use the
92+
* delayFromTime() function to determine the number of discrete delays required
93+
* for a specified amount of time.
94+
* Example :
95+
* @code{.cpp}
96+
* transportDelay<delayFromTime( 2.5f, dt )> myDelay1;
97+
* @endcode
98+
*/
99+
template<size_t numberOfDelays>
100+
class transportDelay {
101+
private:
102+
real_t buf[ numberOfDelays + 1 ];
103+
tdl delay;
104+
public:
105+
/**
106+
* @brief Constructor for the transportDelay class
107+
* @param[in] initValue The output generated by the block between the
108+
* start of the simulation and the Time delay.
109+
*/
110+
transportDelay( const real_t initValue = 0.0_re )
111+
{
112+
static_assert( numberOfDelays >= 1 , "Delay taps should be greater than 0" );
113+
delay.setup( buf, initValue);
114+
}
115+
/**
116+
* @brief Delays the input by a specified amount of time.
117+
* @param[in] xInput The signal to be delayed.
118+
* @return The delayed input signal
119+
*/
120+
real_t operator()( const real_t xInput ) noexcept
121+
{
122+
delay.insertSample( xInput );
123+
return delay.getOldest();
124+
}
125+
};
126+
127+
/**
128+
* @brief Delays the input by a specified amount of samples. You can use this
129+
* class to simulate a discrete time delay.
130+
* @tparam Delay The number of samples to delay the signal.
131+
*/
132+
template<size_t delay>
133+
using discreteDelay = transportDelay<delay>;
134+
135+
73136
/**
74137
* @brief Discrete transfer function for easy LTI system definition
75138
* @note Initial conditions are zero by default.
139+
* @tparam NB Order of the numerator of the transfer function
140+
* @tparam NA Order of the denominator of the transfer function
76141
*/
77142
template<size_t NB, size_t NA>
78143
struct discreteTF {
79144
real_t num[ NB ];
80145
real_t den[ NA ];
81146
discreteStates<(NA>NB)? NA:NB> states = {};
147+
/**
148+
* @brief Constructor for the discreteTF class
149+
* @param[in] numerator An array of size <NB> with the coefficients
150+
* of the numerator in descending powers of z.
151+
* @param[in] denominator An array of size <NA> with the coefficients
152+
* of the denominator in descending powers of z.
153+
*/
82154
discreteTF( const real_t ( &numerator )[ NB ], const real_t ( &denominator)[ NA ] ) {
83155
for ( size_t i = 0; i < NB; ++i ) {
84156
num[ i ] = numerator[ i ];

‎src/include/pid.hpp

Lines changed: 56 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -75,29 +75,58 @@ namespace qlibs {
7575
static const uint32_t UNDEFINED;
7676
protected:
7777
/*! @cond */
78-
real_t p00{ 1.0_re };
79-
real_t p01{ 0.0_re };
80-
real_t p10{ 0.0_re };
81-
real_t p11{ 1.0_re }; /*covariance values*/
82-
real_t b1{ 0.1_re };
83-
real_t a1{ 0.9_re }; /*estimation values*/
84-
real_t uk{ 0.0_re };
85-
real_t yk{ 0.0_re }; /*process I/O measurements*/
86-
real_t l{ 0.9898_re }; /*memory factor [ 0.9 < l < 1 ]*/
87-
real_t il{ 1.0_re };
88-
real_t k{ 1.0_re };
89-
real_t tao{ 1.0_re }; /*process metrics*/
90-
real_t mu{ 0.95_re };
91-
real_t speed{ 0.25_re }; /*fine adjustments [ 0 < mu < speed ] [ 0 < speed < 1 ]*/
92-
uint32_t it{ UNDEFINED };/*enable time*/
78+
real_t p00{ 1.0_re }; /*covariance value*/
79+
real_t p01{ 0.0_re }; /*covariance value*/
80+
real_t p10{ 0.0_re }; /*covariance value*/
81+
real_t p11{ 1.0_re }; /*covariance value*/
82+
real_t b1{ 0.1_re }; /*estimation value*/
83+
real_t a1{ 0.9_re }; /*estimation value*/
84+
real_t uk{ 0.0_re }; /*process input*/
85+
real_t yk{ 0.0_re }; /*process output*/
86+
real_t l{ 0.9898_re }; /*memory factor [ 0.9 < l < 1 ]*/
87+
real_t k{ 1.0_re }; /*process static gain*/
88+
real_t tao{ 1.0_re }; /*process time constant*/
89+
real_t mu{ 0.95_re }; /*variation attenuation*/
90+
real_t speed{ 0.25_re }; /*final controller speed*/
91+
uint32_t it{ UNDEFINED }; /*enable time*/
9392
static bool isValidValue( const real_t x ) noexcept;
9493
pidType type{ pidType::PID_TYPE_PI };
95-
/*! @endcond */
96-
public:
97-
pidAutoTuning() = default;
94+
void initialize( const pidGains current,
95+
const real_t dt ) noexcept;
96+
inline void enable( const uint32_t tEnable ) noexcept
97+
{
98+
it = ( 0UL == tEnable ) ? pidAutoTuning::UNDEFINED : tEnable;
99+
}
100+
inline bool isComplete( void ) const noexcept
101+
{
102+
return ( ( 0UL == it ) && ( it != pidAutoTuning::UNDEFINED ) );
103+
}
104+
inline void setMemoryFactor( const real_t lambda ) noexcept
105+
{
106+
l = lambda;
107+
}
108+
inline void setMomentum( const real_t Mu ) noexcept
109+
{
110+
mu = Mu;
111+
}
112+
inline void setEstimatedControllerSpeed( const real_t alpha ) noexcept
113+
{
114+
speed = alpha;
115+
}
116+
inline void setEstimatedControllerType( const pidType t ) noexcept
117+
{
118+
type = t;
119+
}
120+
static inline bool isValidParam( const real_t p ) noexcept
121+
{
122+
return ( p > 0.0_re ) && ( p <= 1.0_re );
123+
}
98124
bool step( const real_t u,
99125
const real_t y,
100126
const real_t dt ) noexcept;
127+
/*! @endcond */
128+
public:
129+
pidAutoTuning() = default;
101130
pidGains getEstimates( void ) const noexcept;
102131
};
103132

@@ -107,7 +136,6 @@ namespace qlibs {
107136
*/
108137
class pidController : public pidGains, public nState, private nonCopyable {
109138
private:
110-
//real_t Kc, Ki, Kd;
111139
real_t b, c, sat_Min, sat_Max, epsilon, kw, kt, D, u1, beta, uSat;
112140
real_t dt{ 1.0_re };
113141
real_t m, mInput;
@@ -226,11 +254,19 @@ namespace qlibs {
226254

227255
/**
228256
* @brief Set the tuning parameter for the derivative filter.
229-
* @param[in] Beta The tuning parameter. [ 0 < Beta < 1 ]
257+
* @param[in] Beta The tuning parameter. [ 0 <= Beta < 1 ]
230258
* @return @c true on success, otherwise return @c false.
231259
*/
232260
bool setDerivativeFilter( const real_t Beta ) noexcept;
233261

262+
/**
263+
* @brief Set the time constant for the derivative filter.
264+
* @note
265+
* @param[in] Tf Derivative filter time constant [ Tf >= 0 ]
266+
* @return @c true on success, otherwise return @c false.
267+
*/
268+
bool setDerivativeFilterTimeConstant( const real_t Tf ) noexcept;
269+
234270
/**
235271
* @brief Change the controller operational mode.
236272
* In pidMode::PID_AUTOMATIC, the computed output of the PID controller

0 commit comments

Comments
 (0)