diff --git a/.gitignore b/.gitignore index 368f69766cf601dd0e3f9e85107d5ac2090093eb..d8997c3007d8b78f909a9478301c241c7c9209d4 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 diff --git a/tools/controller/pid.c b/tools/controller/pid.c deleted file mode 100644 index aebc00a534dd863e24671069b107207194e158de..0000000000000000000000000000000000000000 --- 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 new file mode 100644 index 0000000000000000000000000000000000000000..75d03ebf47fbefcccd098b151750d1d933c0338a --- /dev/null +++ b/tools/controller/pid.cpp @@ -0,0 +1,112 @@ +/****************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); +} + +//前馈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) +{ + 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; +} + diff --git a/tools/include/pid.h b/tools/include/pid.h index 252e1648467987cc89a8f6ea8ddb59b9337a7f76..53a2355a9dc30d48562e95cdeba08c8560711e1d 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