Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F65206556
pid.c
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Sat, Jun 1, 20:38
Size
1 KB
Mime Type
text/x-c
Expires
Mon, Jun 3, 20:38 (2 d)
Engine
blob
Format
Raw Data
Handle
17999901
Attached To
R2671 HHRI-software
pid.c
View Options
#include "pid.h"
#include "utils.h"
/**
* @brief Initialize the PID structure.
* @param pid: pointer to the PID structure.
* @param kp: proportionnal coefficient of the PID. No effect if zero.
* @param ki: integral coefficient of the PID. No effect if zero.
* @param kd: derivative coefficient of the PID. No effect if zero.
* @param arw: maximum value of the integrator (anti-reset windup). Disabled if negative.
* @param feedforward: component proportional to the target (not the error). No effect if zero.
*/
void
pid_Init
(
pid_Pid
*
pid
,
float32_t
kp
,
float32_t
ki
,
float32_t
kd
,
float32_t
arw
,
float32_t
feedforward
)
{
pid
->
kp
=
kp
;
pid
->
ki
=
ki
;
pid
->
kd
=
kd
;
pid
->
arw
=
arw
;
pid
->
previousErr
=
0.0f
;
pid
->
integrator
=
0.0f
;
pid
->
feedforward
=
feedforward
;
}
/**
* @brief Step the PID structure.
* @param pid: pointer to the PID structure.
* @param current: current state of the system to control.
* @param target: target state of the system to control.
* @param dt: timestep (time since the last call of this function) [s].
* @retval command to apply to the system.
*/
float32_t
pid_Step
(
pid_Pid
*
pid
,
float32_t
current
,
float32_t
target
,
float32_t
dt
)
{
float32_t
err
;
pid
->
current
=
current
;
pid
->
target
=
target
;
// Error computation.
err
=
target
-
current
;
// Feedforward part.
pid
->
command
=
pid
->
feedforward
*
target
;
// Proportionnal part.
pid
->
command
+=
pid
->
kp
*
err
;
// Integral part.
if
(
pid
->
ki
>
0.0f
)
{
pid
->
integrator
+=
pid
->
ki
*
(
err
*
dt
);
if
(
pid
->
arw
>
0.0f
)
utils_SaturateF
(
&
pid
->
integrator
,
-
pid
->
arw
,
pid
->
arw
);
pid
->
command
+=
pid
->
integrator
;
}
// Derivative part.
pid
->
command
+=
pid
->
kd
*
((
err
-
pid
->
previousErr
)
/
dt
);
pid
->
previousErr
=
err
;
return
pid
->
command
;
}
Event Timeline
Log In to Comment