Skip to content

Commit faf6ecf

Browse files
author
camilo
committed
fix pid epsilon
1 parent b3ab125 commit faf6ecf

File tree

7 files changed

+90
-15
lines changed

7 files changed

+90
-15
lines changed

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.6",
19+
"version": "1.2.7",
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.6
2+
version=1.2.7
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.6
3+
VERSION 1.2.7
44
DESCRIPTION "A collection of useful C++ libraries for embedded systems"
55
LANGUAGES CXX )
66

src/include/algorithm.hpp

Lines changed: 70 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -87,12 +87,20 @@ namespace qlibs {
8787
* @param[in,out] array The array to be sorted.
8888
* @param[in] first Initial position of the portion to be sorted
8989
* @param[in] last Final position of the portion to be sorted
90+
* @param[in] comp Comparison function which returns ​@c true if the first
91+
* argument is less than (i.e. is ordered before) the second.
92+
* The signature of the comparison function should be equivalent to the
93+
* following: bool cmp(const Type1& a, const Type2& b);
94+
* @code{.c}
95+
* bool cmp( const T& a, const T& b );
96+
* @endcode
9097
* @return none.
9198
*/
9299
template<typename T, size_t n>
93100
void sort( T ( &array )[ n ],
94101
size_t first = 0U,
95-
size_t last = n - 1U ) noexcept
102+
size_t last = n - 1U,
103+
bool (*comp)( const T&, const T&) = nullptr ) noexcept
96104
{
97105
if ( n > 1U ) {
98106
algorithm::impl::sort_stack<impl::sort_pair, n> stack;
@@ -109,7 +117,13 @@ namespace qlibs {
109117
T pivotValue = array[ end ];
110118

111119
for ( int i = start; i < end; ++i ) {
112-
if ( array[ i ] < pivotValue ) {
120+
if ( nullptr != comp ) {
121+
if ( comp( array[ i ], array[ pivotIndex ] ) ) {
122+
algorithm::swap( array[ i ], array[ pivotIndex ] );
123+
++pivotIndex;
124+
}
125+
}
126+
else if ( array[ i ] < pivotValue ) {
113127
algorithm::swap( array[ i ], array[ pivotIndex ] );
114128
++pivotIndex;
115129
}
@@ -132,7 +146,6 @@ namespace qlibs {
132146
* @param[in] last Final position of the portion to reverse
133147
* @return none.
134148
*/
135-
136149
template<typename T, size_t n>
137150
inline void reverse( T ( &array )[ n ],
138151
const size_t first = 0U,
@@ -157,7 +170,6 @@ namespace qlibs {
157170
* @param[in] k Positions to rotate. Sign determines the rotate direction.
158171
* @return none.
159172
*/
160-
161173
template<typename T, size_t n>
162174
void rotate( T ( &array )[ n ],
163175
const int k = 1 ) noexcept
@@ -190,6 +202,7 @@ namespace qlibs {
190202
* @param[in] value The value to set all elements to.
191203
* @param[in] first Initial position of the portion to fill
192204
* @param[in] last Final position of the portion to fill
205+
* @return none.
193206
*/
194207
template<typename T, size_t n>
195208
inline void fill( T ( &array )[ n ],
@@ -293,6 +306,59 @@ namespace qlibs {
293306
return ret;
294307
}
295308

309+
/**
310+
* @brief Replaces all elements satisfying specific criteria with
311+
* @a new_value in the range [first, last). Replaces all elements that
312+
* are equal to old_value (using operator==)
313+
* @param[in] array The array where the check is performed
314+
* @param[in] old_value The value of elements to replace
315+
* @param[in] new_value The value to use as replacement
316+
* @param[in] first Initial position of the portion to check
317+
* @param[in] last Final position of the portion to check
318+
* @return @c none.
319+
*/
320+
template<typename T, size_t n>
321+
inline void replace( T ( &array )[ n ],
322+
const T& old_value,
323+
const T& new_value,
324+
const size_t first = 0U,
325+
const size_t last = n - 1U ) noexcept
326+
{
327+
for ( size_t i = first; i <= last; ++i ) {
328+
if ( old_value == array[ i ] ) {
329+
array[ i ] = new_value;
330+
break;
331+
}
332+
}
333+
}
334+
335+
/**
336+
* @brief Replaces all elements satisfying specific criteria with
337+
* @a new_value in the range [first, last). Replaces all elements for
338+
* which predicate @a pred returns @c true
339+
* @param[in] array The array where the check is performed
340+
* @param[in] pred Unary predicate which returns @c true if the element
341+
* value should be replaced.
342+
* @param[in] new_value The value to use as replacement
343+
* @param[in] first Initial position of the portion to check
344+
* @param[in] last Final position of the portion to check
345+
* @return @c none.
346+
*/
347+
template<typename T, size_t n>
348+
inline bool replace_if( T ( &array )[ n ],
349+
bool (*pred)( const T& ),
350+
const T& new_value,
351+
const size_t first = 0U,
352+
const size_t last = n - 1U ) noexcept
353+
{
354+
for ( size_t i = first; i <= last; ++i ) {
355+
if ( pred( array[ i ] ) ) {
356+
array[ i ] = new_value;
357+
break;
358+
}
359+
}
360+
}
361+
296362
/**
297363
* @brief Returns the number of elements in the range [first,last) for
298364
* which @c pred is @c true.

src/include/pid.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -119,6 +119,7 @@ namespace qlibs {
119119
pidMode mode{ pidMode::PID_AUTOMATIC };
120120
pidDirection dir{ pidDirection::PID_FORWARD };
121121
bool isInitialized{ false };
122+
real_t error( real_t w, real_t y, real_t k = 1.0_re ) noexcept;
122123
static real_t saturate( real_t x,
123124
const real_t vMin,
124125
const real_t vMax ) noexcept;

src/pid.cpp

Lines changed: 14 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -246,6 +246,17 @@ bool pidController::removeModelReferenceControl( void ) noexcept
246246
return retValue;
247247
}
248248
/*============================================================================*/
249+
real_t pidController::error( real_t w, real_t y, real_t k ) noexcept
250+
{
251+
real_t e = ( k*w ) - y;
252+
253+
if ( ffmath::absf( e ) <= epsilon ) {
254+
e = 0.0_re;
255+
}
256+
257+
return e;
258+
}
259+
/*============================================================================*/
249260
real_t pidController::control( const real_t w,
250261
const real_t y ) noexcept
251262
{
@@ -261,14 +272,11 @@ real_t pidController::control( const real_t w,
261272
ki = ( ki > 0.0_re ) ? -ki : ki;
262273
kd = ( kd > 0.0_re ) ? -kd : kd;
263274
}
264-
e = w - y;
265-
if ( ffmath::absf( e ) <= epsilon ) {
266-
e = 0.0_re;
267-
}
268-
de = derive( ( c*w ) - y , dt, false );
275+
e = error( w, y );
276+
de = derive( error( w, y, c ) , dt, false );
269277
ie = integrate( e + u1 , dt );
270278
D = de + beta*( D - de ); /*derivative filtering*/
271-
v = ( kc*( ( b*w ) - y ) ) + ( ki*ie ) + ( kd*D ); /*compute PID action*/
279+
v = ( kc*error( w, y, b ) ) + ( ki*ie ) + ( kd*D ); /*compute PID action*/
272280
if ( nullptr != yr ) {
273281
/*MRAC additive controller using the modified MIT rule*/
274282
real_t theta = 0.0_re;

src/qlibs.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -41,8 +41,8 @@ This file is part of the QuarkTS++ OS distribution.
4141
#ifndef QLIBS_CPP_H
4242
#define QLIBS_CPP_H
4343

44-
#define QLIBS_CPP_VERSION "1.2.6"
45-
#define QLIBS_CPP_VERNUM ( 126U )
44+
#define QLIBS_CPP_VERSION "1.2.7"
45+
#define QLIBS_CPP_VERNUM ( 127U )
4646
#define QLIBS_CPP_CAPTION "qLibs++" QLIBS_CPP_VERSION
4747

4848
#include <include/qlibs_types.hpp>

0 commit comments

Comments
 (0)