One Framework 0.1.0
RoboMaster嵌入式框架“一键”解决方案，为你的“创意”服务。
载入中...
搜索中...
未找到
heater_pid.h
浏览该文件的文档.
1// C
2#ifndef OF_HEATER_PID_H
3#define OF_HEATER_PID_H
4
5#include <math.h>
6
7#ifdef __cplusplus
8extern "C" {
9#endif
10
11typedef struct
12{
13 float Kp;
14 float Ki;
15 float Kd;
16 float IntegralLimit; /* limit for integral term */
17 float MaxOutput; /* maximum absolute output */
18 float Deadband; /* error deadband (absolute) */
19} PID_Params_t;
20
21/* Simplified, concrete PID object (no dynamic allocation) */
22typedef struct PID_Obj_t
23{
24 PID_Params_t p;
25 float prev_error;
26 float integral;
27} PID_t;
28
29/* Initialize a PID object with provided parameters. */
30static inline void PID_Init(PID_t* pid, const PID_Params_t* params)
31{
32 if (pid == NULL || params == NULL)
33 {
34 return;
35 }
36 pid->p = *params;
37 pid->prev_error = 0.0f;
38 pid->integral = 0.0f;
39}
40
41/* Reset integral and derivative state */
42static inline void PID_Reset(PID_t* pid)
43{
44 if (pid == NULL)
45 {
46 return;
47 }
48 pid->prev_error = 0.0f;
49 pid->integral = 0.0f;
50}
51
52/* Compute PID output. Operates on the provided PID_t object. */
53static inline float PID_Compute(PID_t* pid, float setpoint, float measurement)
54{
55 if (pid == NULL)
56 {
57 return 0.0f;
58 }
59
60 float error = setpoint - measurement;
61
62 if (fabsf(error) <= pid->p.Deadband)
63 {
64 /* in deadband, do not integrate or change previous error */
65 return 0.0f;
66 }
67
68 /* simple discrete integration (assumes fixed call interval) */
69 pid->integral += error;
70 if (pid->integral > pid->p.IntegralLimit)
71 {
72 pid->integral = pid->p.IntegralLimit;
73 }
74 else if (pid->integral < -pid->p.IntegralLimit)
75 {
76 pid->integral = -pid->p.IntegralLimit;
77 }
78
79 float derivative = error - pid->prev_error;
80
81 float output = pid->p.Kp * error
82 + pid->p.Ki * pid->integral
83 + pid->p.Kd * derivative;
84
85 /* clamp output to configured max magnitude */
86 if (output > pid->p.MaxOutput)
87 {
88 output = pid->p.MaxOutput;
89 }
90 else if (output < -pid->p.MaxOutput)
91 {
92 output = -pid->p.MaxOutput;
93 }
94
95 pid->prev_error = error;
96 return output;
97}
98
99#ifdef __cplusplus
100}
101#endif
102
103#endif // OF_HEATER_PID_H
PID_Init
static void PID_Init(PID_t *pid, const PID_Params_t *params)
定义 heater_pid.h:30
PID_Compute
static float PID_Compute(PID_t *pid, float setpoint, float measurement)
定义 heater_pid.h:53
PID_t
struct PID_Obj_t PID_t
PID_Reset
static void PID_Reset(PID_t *pid)
定义 heater_pid.h:42
PID_Obj_t
定义 heater_pid.h:23
PID_Obj_t::integral
float integral
定义 heater_pid.h:26
PID_Obj_t::p
PID_Params_t p
定义 heater_pid.h:24
PID_Obj_t::prev_error
float prev_error
定义 heater_pid.h:25
PID_Params_t
定义 heater_pid.h:12
PID_Params_t::Kp
float Kp
定义 heater_pid.h:13
PID_Params_t::IntegralLimit
float IntegralLimit
定义 heater_pid.h:16
PID_Params_t::Ki
float Ki
定义 heater_pid.h:14
PID_Params_t::Deadband
float Deadband
定义 heater_pid.h:18
PID_Params_t::Kd
float Kd
定义 heater_pid.h:15
PID_Params_t::MaxOutput
float MaxOutput
定义 heater_pid.h:17