From 265263ae2823b72baed8fb7afd2459d7502bc789 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=84=95=E7=BC=A4=20=E5=91=A8?= <431433788@qq.com> Date: Thu, 24 Aug 2023 14:28:19 +0800 Subject: [PATCH 1/4] 1 --- tools/controller/pid.cpp | 74 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 tools/controller/pid.cpp diff --git a/tools/controller/pid.cpp b/tools/controller/pid.cpp new file mode 100644 index 0000000..c5d51ef --- /dev/null +++ b/tools/controller/pid.cpp @@ -0,0 +1,74 @@ +/****************PID运算****************/ + +#include "../include/pid.h" + + +//初始化pid参数 +PID::PID(ConfItem* conf) : + kp(Conf_GetValue(conf, "p", float, 0)), + ki(ki=Conf_GetValue(conf, "i", float, 0)), + kd(Conf_GetValue(conf, "d", float, 0)), + maxIntegral(Conf_GetValue(conf, "max-i", float, 0)), + maxOutput(Conf_GetValue(conf, "max-out", float, 0)), + deadzone(0) +{ + +} + +//单级pid计算 +void PID::SingleCalc(float reference,float feedback) +{ + //更新数据 + lastError = error; + if(ABS(reference - feedback) < deadzone)//若误差在死区内则error直接置0 + error = 0; + else + error = reference - feedback; + //计算微分 + output = (error - lastError) * kd; + //计算比例 + output += error * kp; + //计算积分 + integral += error * ki; + LIMIT(integral, -maxIntegral, maxIntegral);//积分限幅 + output += integral; + //输出限幅 + LIMIT(output, -maxOutput, maxOutput); +} + +void PID::FeedbackCalc() + +//串级pid计算 +void CascadePID::CascadeCalc(float angleRef,float angleFdb,float speedFdb) +{ + outer.SingleCalc(angleRef, angleFdb);//计算外环(角度环) + inner.SingleCalc(outer.output, speedFdb);//计算内环(速度环) + output = inner.output; +} + +//清空一个pid的历史数据 +void PID::Clear() +{ + error=0; + lastError=0; + integral=0; + output=0; +} + +//重新设定pid输出限幅 +void PID::SetMaxOutput(float maxOutput) +{ + this->maxOutput = maxOutput; +} + +void PID::SetMaxIntegral(float maxIntegral) +{ + this->maxIntegral = maxIntegral; +} + +//设置PID死区 +void PID::SetDeadzone(float deadzone) +{ + this->deadzone = deadzone; +} + -- Gitee From 473c563fc61e5b157133084e2b4aac6152336cc2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=84=95=E7=BC=A4=20=E5=91=A8?= <431433788@qq.com> Date: Thu, 24 Aug 2023 14:54:08 +0800 Subject: [PATCH 2/4] =?UTF-8?q?=E7=BC=96=E5=86=99=E4=BA=86=E8=BE=85?= =?UTF-8?q?=E5=8A=A9=E7=94=B5=E6=8E=A7=E7=90=86=E8=A7=A3=E7=9A=84=E6=B3=A8?= =?UTF-8?q?=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .idea/workspace.xml | 94 ++++++++++++++++++++++++++++++++++++++++ tools/controller/pid.c | 64 --------------------------- tools/controller/pid.cpp | 50 ++++++++++++++++++--- tools/include/pid.h | 50 ++++++++++++--------- 4 files changed, 167 insertions(+), 91 deletions(-) create mode 100644 .idea/workspace.xml delete mode 100644 tools/controller/pid.c diff --git a/.idea/workspace.xml b/.idea/workspace.xml new file mode 100644 index 0000000..ad4f845 --- /dev/null +++ b/.idea/workspace.xml @@ -0,0 +1,94 @@ + + + + + + + { + "useNewFormat": true +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1692777502148 + + + + + + + + + + + + \ No newline at end of file diff --git a/tools/controller/pid.c b/tools/controller/pid.c deleted file mode 100644 index aebc00a..0000000 --- a/tools/controller/pid.c +++ /dev/null @@ -1,64 +0,0 @@ -/****************PID运算****************/ - -#include "pid.h" - -//初始化pid参数 -void PID_Init(PID *pid, ConfItem* conf) -{ - pid->kp=Conf_GetValue(conf, "p", float, 0); - pid->ki=Conf_GetValue(conf, "i", float, 0); - pid->kd=Conf_GetValue(conf, "d", float, 0); - pid->maxIntegral=Conf_GetValue(conf, "max-i", float, 0); - pid->maxOutput=Conf_GetValue(conf, "max-out", float, 0); - pid->deadzone=0; -} - -//单级pid计算 -void PID_SingleCalc(PID *pid,float reference,float feedback) -{ - //更新数据 - pid->lastError=pid->error; - if(ABS(reference-feedback) < pid->deadzone)//若误差在死区内则error直接置0 - pid->error=0; - else - pid->error=reference-feedback; - //计算微分 - pid->output=(pid->error-pid->lastError)*pid->kd; - //计算比例 - pid->output+=pid->error*pid->kp; - //计算积分 - pid->integral+=pid->error*pid->ki; - LIMIT(pid->integral,-pid->maxIntegral,pid->maxIntegral);//积分限幅 - pid->output+=pid->integral; - //输出限幅 - LIMIT(pid->output,-pid->maxOutput,pid->maxOutput); -} - -//串级pid计算 -void PID_CascadeCalc(CascadePID *pid,float angleRef,float angleFdb,float speedFdb) -{ - PID_SingleCalc(&pid->outer,angleRef,angleFdb);//计算外环(角度环) - PID_SingleCalc(&pid->inner,pid->outer.output,speedFdb);//计算内环(速度环) - pid->output=pid->inner.output; -} - -//清空一个pid的历史数据 -void PID_Clear(PID *pid) -{ - pid->error=0; - pid->lastError=0; - pid->integral=0; - pid->output=0; -} - -//重新设定pid输出限幅 -void PID_SetMaxOutput(PID *pid,float maxOut) -{ - pid->maxOutput=maxOut; -} - -//设置PID死区 -void PID_SetDeadzone(PID *pid,float deadzone) -{ - pid->deadzone=deadzone; -} diff --git a/tools/controller/pid.cpp b/tools/controller/pid.cpp index c5d51ef..75d03eb 100644 --- a/tools/controller/pid.cpp +++ b/tools/controller/pid.cpp @@ -16,7 +16,7 @@ PID::PID(ConfItem* conf) : } //单级pid计算 -void PID::SingleCalc(float reference,float feedback) +void PID::SingleCalc(float reference, float feedback) { //更新数据 lastError = error; @@ -36,7 +36,45 @@ void PID::SingleCalc(float reference,float feedback) LIMIT(output, -maxOutput, maxOutput); } -void PID::FeedbackCalc() +//前馈pid计算 +/* +当逻辑不复杂，没必要或不想给函数起名的时候，我们会推荐用Lambda表达式来代替函数指针Lambda表达式的结构 +[capture-list] (params) -> ret(optional) { body } +captures: 捕捉子句，作用是捕捉外部变量 +[]，啥都不捕捉，也就是Lambda表达式body中不能出现没在body中声明的变量 +[=]，按值捕捉所有变里，也就是lambda表达式前的所有变里，都可以以值传递(拷贝)的方式出现在body中（性能问题，不建议使用） +[&]，按引用捕捉所有变量，也就是Lambda表达式前的所有变量，都可以以引用传递的方式出现在body中（性能问题，不建议使用） +[&a]，按引用捕捉变量a，按值捕捉其他变量 +[&，a]，按值捕捉变量a，按引用捕捉其他变量 +[=，a]，[&，&a]这样的写法是错误的 +params: 和普通函数一样的参数(设置本地参数) +ret: 返回值类型，也可以省略，让编译器通过 return 语句自动推导 +body: 函数的具体逻辑 +样例： + [] (int a) ->float { return (a * 10);}; + a * 10可以改成自己所需要的数学模型，然后将lambda表达式整体传入FeedbackCalc函数中， +*/ +void PID::FeedbackCalc(float reference, float feedback, const std::function& lambda) +{ + //更新数据 + lastError = error; + if(ABS(reference - feedback) < deadzone)//若误差在死区内则error直接置0 + error = 0; + else + error = reference - feedback; + //计算微分 + output = (error - lastError) * kd; + //计算比例 + output += error * kp; + //计算积分 + integral += error * ki; + LIMIT(integral, -maxIntegral, maxIntegral);//积分限幅 + output += integral; + //加入前馈 + output += lambda(reference); + //输出限幅 + LIMIT(output, -maxOutput, maxOutput); +} //串级pid计算 void CascadePID::CascadeCalc(float angleRef,float angleFdb,float speedFdb) @@ -49,10 +87,10 @@ void CascadePID::CascadeCalc(float angleRef,float angleFdb,float speedFdb) //清空一个pid的历史数据 void PID::Clear() { - error=0; - lastError=0; - integral=0; - output=0; + error = 0; + lastError = 0; + integral = 0; + output = 0; } //重新设定pid输出限幅 diff --git a/tools/include/pid.h b/tools/include/pid.h index 252e164..53a2355 100644 --- a/tools/include/pid.h +++ b/tools/include/pid.h @@ -1,7 +1,9 @@ #ifndef _USER_PID_H_ #define _USER_PID_H_ -#include "config.h" +#include "../../conf/config.h" + +#include #ifndef LIMIT #define LIMIT(x,min,max) (x)=(((x)<=(min))?(min):(((x)>=(max))?(max):(x))) @@ -11,27 +13,33 @@ #define ABS(x) ((x)>=0?(x):-(x)) #endif -typedef struct _PID +class PID { - float kp,ki,kd; - float error,lastError;//误差、上次误差 - float integral,maxIntegral;//积分、积分限幅 - float output,maxOutput;//输出、输出限幅 - float deadzone;//死区 -}PID; - -typedef struct _CascadePID +public: + float kp,ki,kd; + float error,lastError;//误差、上次误差 + float integral,maxIntegral;//积分、积分限幅 + float output,maxOutput;//输出、输出限幅 + float deadzone;//死区 + + PID(ConfItem* conf); + void SingleCalc(float reference,float feedback); + void FeedbackCalc(float reference, float feedback, const std::function& lambda); + void Clear(); + void SetMaxOutput(float maxOutput); + void SetMaxIntegral(float maxIntegral); + void SetDeadzone(float deadzone); +}; + +class CascadePID { - PID inner;//内环 - PID outer;//外环 - float output;//串级输出，等于inner.output -}CascadePID; - -void PID_Init(PID *pid, ConfItem* conf); -void PID_SingleCalc(PID *pid,float reference,float feedback); -void PID_CascadeCalc(CascadePID *pid,float angleRef,float angleFdb,float speedFdb); -void PID_Clear(PID *pid); -void PID_SetMaxOutput(PID *pid,float maxOut); -void PID_SetDeadzone(PID *pid,float deadzone); +public: + PID inner;//内环 + PID outer;//外环 + float output;//串级输出，等于inner.output + + void CascadeCalc(float angleRef,float angleFdb,float speedFdb); +}; + #endif -- Gitee From f084a0b2a9331b983346388601b4d171d3f6a474 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=84=95=E7=BC=A4=20=E5=91=A8?= <431433788@qq.com> Date: Thu, 24 Aug 2023 21:45:49 +0800 Subject: [PATCH 3/4] =?UTF-8?q?=E6=9B=B4=E6=96=B0gitignore?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 368f697..d8997c3 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ .vscode/ .cache/ -build/ \ No newline at end of file +build/ +.idea/ \ No newline at end of file -- Gitee From 3f182a7a5ab4134fdd658f7d4e898dfb6de5470d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=84=95=E7=BC=A4=20=E5=91=A8?= <431433788@qq.com> Date: Thu, 24 Aug 2023 21:47:30 +0800 Subject: [PATCH 4/4] 111 --- .idea/workspace.xml | 94 --------------------------------------------- 1 file changed, 94 deletions(-) delete mode 100644 .idea/workspace.xml diff --git a/.idea/workspace.xml b/.idea/workspace.xml deleted file mode 100644 index ad4f845..0000000 --- a/.idea/workspace.xml +++ /dev/null @@ -1,94 +0,0 @@ - - - - - - - { - "useNewFormat": true -} - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1692777502148 - - - - - - - - - - - - \ No newline at end of file -- Gitee