From 0c1414599cd8de32928445b31fe5723c6c64795e Mon Sep 17 00:00:00 2001 From: DogeQaQ <1350787318@qq.com> Date: Mon, 9 Oct 2023 20:33:46 +0800 Subject: [PATCH] try --- C_PID/Doge-ai/follow/main.c | 62 +++++++++++++++++++++ C_PID/Doge-ai/follow/pid.c | 105 ++++++++++++++++++++++++++++++++++++ C_PID/Doge-ai/follow/pid.h | 37 +++++++++++++ C_PID/Doge-ai/try/main.c | 49 +++++++++++++++++ C_PID/Doge-ai/try/pid.c | 63 ++++++++++++++++++++++ C_PID/Doge-ai/try/pid.h | 24 +++++++++ 6 files changed, 340 insertions(+) create mode 100644 C_PID/Doge-ai/follow/main.c create mode 100644 C_PID/Doge-ai/follow/pid.c create mode 100644 C_PID/Doge-ai/follow/pid.h create mode 100644 C_PID/Doge-ai/try/main.c create mode 100644 C_PID/Doge-ai/try/pid.c create mode 100644 C_PID/Doge-ai/try/pid.h diff --git a/C_PID/Doge-ai/follow/main.c b/C_PID/Doge-ai/follow/main.c new file mode 100644 index 0000000..c534b7b --- /dev/null +++ b/C_PID/Doge-ai/follow/main.c @@ -0,0 +1,62 @@ +//这是找了代码学习了下，稍微调了一下和做了些注释，因为自己试着做的好像很不对劲（ + +#include +#include "pid.h" +#include "pid.c" +#include + +int main() +{ + PID pid1, pid2, pid3; + float kp, ki, kd, pos_output, inc_output, ff_output, ff_mid_output; + + // 初始化PID控制器 + PID_Init(&pid1); + PID_Init(&pid2); + PID_Init(&pid3); + + + printf("输入本次使用的PID参数:\n"); + printf("KP: "); + scanf("%f", &kp); + printf("KI: "); + scanf("%f", &ki); + printf("KD: "); + scanf("%f", &kd); + printf("普通位置式pid\t普通增量式pid\t前馈增量式pid\n误差表:\n"); + + // 设置PID参数 + PID_SetParameter(&pid1, kp, ki, kd); + PID_SetParameter(&pid2, kp, ki, kd); + PID_SetParameter(&pid3, kp, ki, kd); + pid1.target = 100; + pid1.max_integral = 100; + pid1.max_output = 200; + pid2.target = 100; + pid2.max_integral = 100; + pid2.max_output = 200; + pid3.target = 100; + pid3.max_integral = 100; + pid3.max_output = 200; + ff_mid_output = pid3.output; + + // 模拟PID控制过程 + for (int i = 0; i < 100; i++) { + + // 普通位置式PID + PID_PostionalPID(&pid1); + pid1.current = pid1.output; + + // 普通增量式PID + PID_IncrementalPID(&pid2); + pid2.current += pid2.output; + + // 前馈增量式PID + PID_IncrementalPID(&pid3); + ff_mid_output += pid3.output; + pid3.current = FeedForward(pid3.target) + ff_mid_output; + printf("%.2f\t\t%.2f\t\t%.2f\n", pid1.error, pid2.error, pid3.error); + } + system("pause"); + return 0; +} diff --git a/C_PID/Doge-ai/follow/pid.c b/C_PID/Doge-ai/follow/pid.c new file mode 100644 index 0000000..52c7b58 --- /dev/null +++ b/C_PID/Doge-ai/follow/pid.c @@ -0,0 +1,105 @@ +#include "pid.h" +#include + +/************************************************************************* + * 函数名称：PID_Init + * 功能说明：PID初始化 + * 参数说明：pid: PID结构体指针 + * 函数返回：0 + *************************************************************************/ +int PID_Init(PID *pid) { + pid->kp = 1; + pid->ki = 0; + pid->kd = 0; + pid->p_out = 0; + pid->i_out = 0; + pid->d_out = 0; + pid->output = 0; + pid->target = 0; + pid->current = 0; + pid->error = 0; + pid->last_error = 0; + pid->pre_last_error = 0; + pid->i_band = 1000; + pid->max_integral = 1000; + pid->max_output = 1000; + return 0; +} + +/************************************************************************* + * 函数名称：PID_SetParameter + * 功能说明：PID参数设置 + * 参数说明：pid: PID结构体指针 + * kp: 比例系数 + * ki: 积分系数 + * kd: 微分系数 + * 函数返回：0 + *************************************************************************/ +int PID_SetParameter(PID *pid, float kp, float ki, float kd) { + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + return 0; +} + +/************************************************************************* + * 函数名称：PID_PostionalPID + * 功能说明：位置式PID计算 + * 参数说明：pid: PID结构体指针 + * 函数返回：0 + *************************************************************************/ +int PID_PostionalPID(PID *pid) { + pid->error = pid->target - pid->current; + pid->p_out = pid->kp * pid->error; + // 积分分离 + if(fabs(pid->current) <= pid->i_band){ + pid->i_out += pid->ki * pid->error; + // 积分限幅 + LIMIT(pid->i_out, pid->max_integral); + } + else pid->i_out = 0; + pid->d_out = pid->kd * (pid->error - pid->last_error); + pid->output = pid->p_out + pid->i_out + pid->d_out; + // 输出限幅 + LIMIT(pid->output, pid->max_output); + pid->last_error = pid->error; + return 0; +} + +/************************************************************************* + * 函数名称：PID_IncrementalPID + * 功能说明：普通增量式PID计算 + * 参数说明：pid: PID结构体指针 + * 函数返回：0 + *************************************************************************/ +int PID_IncrementalPID(PID *pid){ + pid->error = pid->target - pid->current; + pid->p_out = pid->kp * (pid->error - pid->last_error); + // 积分分离 + if(fabs(pid->current) <= pid->i_band){ + pid->i_out = pid->ki * pid->error; + // 积分限幅 + LIMIT(pid->i_out, pid->max_integral); + } + else pid->i_out = 0; + pid->d_out = pid->kd * (pid->error - 2 * pid->last_error + pid->pre_last_error); + pid->output = pid->p_out + pid->i_out + pid->d_out; + // 输出限幅 + LIMIT(pid->output, pid->max_output); + pid->pre_last_error = pid->last_error; + pid->last_error = pid->error; + return 0; +} + +/************************************************************************* + * 函数名称：FeedForward + * 功能说明：根据目标与预测模型前馈控制 + * 参数说明：target: 目标值 + * 函数返回：前馈输出值 + *************************************************************************/ +float FeedForward(float target) +{ + //这里简单预测为线性模型 + float kf = 0.5; + return kf * target; +} diff --git a/C_PID/Doge-ai/follow/pid.h b/C_PID/Doge-ai/follow/pid.h new file mode 100644 index 0000000..ce43327 --- /dev/null +++ b/C_PID/Doge-ai/follow/pid.h @@ -0,0 +1,37 @@ +#ifndef PID_H +#define PID_H//多次用 +#define LIMIT(TargetValue, LimitValue) \ +if (TargetValue > LimitValue)\ +{\ + TargetValue = LimitValue;\ +}\ +else if (TargetValue < -LimitValue)\ +{\ + TargetValue = -LimitValue;\ +}//限幅 + +typedef struct { + float kp; // 比例系数 + float ki; // 积分系数 + float kd; // 微分系数 + float p_out; // 比例输出 + float i_out; // 积分输出 + float d_out; // 微分输出 + float output; // 输出值 + float target; // 设定值 + float current; // 当前值 + float error; // 误差 + float last_error; // 上一次的误差 + float pre_last_error; // 上上次的误差 + float i_band; // 积分分离值 + float max_integral; // 最大积分值 + float max_output; // 最大输出值 +} PID;//各项数据封装为一个结构体 + +int PID_Init(PID *pid);//初始化 +int PID_SetParameter(PID *pid, float kp, float ki, float kd);//设置参数 +int PID_PostionalPID(PID *pid);//位置式PID计算 +int PID_IncrementalPID(PID *pid);//增量式PID计算 +float FeedForward(float target);//前馈式 + +#endif /* PID_H */ diff --git a/C_PID/Doge-ai/try/main.c b/C_PID/Doge-ai/try/main.c new file mode 100644 index 0000000..11515f8 --- /dev/null +++ b/C_PID/Doge-ai/try/main.c @@ -0,0 +1,49 @@ +//自己从零尝试做了，但是具体公式的实现和模拟的过程没弄明白（甚至感觉因为看了蛮多不同的代码弄混了） +//另外找了一个代码研究和注释，再研究研究先，后面再尝试弄自己的，怕时间到了就打pr了，也顺便把这个失败品发了（ +#include +#include"pid.h" +#include"pid.c" +int main() +{ + PID pid1,pid2; + PID_Init(&pid1); + PID_Init(&pid2); + + float kp,ki,kd; + printf("输入本次使用的PID参数\n"); + printf("KP:"); + scanf("%lf",&kp); + printf("KI:"); + scanf("%lf",&ki); + printf("KD:"); + scanf("%lf",&kd); + + PID_SetParameter(kp,ki,kd,&pid1); + PID_SetParameter(kp,ki,kd,&pid2); + + float target; + printf("请输入目标值:"); + scanf("%lf",&target); + float actual; + printf("请输入当前值:"); + scanf("%lf",&actual); + + float actual1,actual2; + actual1=target; + actual2=target; + + int times =0; + PID_Init(&pid1); + PID_Init(&pid1); + printf("普通位置式pid误差表: 普通增量式pid误差表:\n"); + for(times = 0 ; times<10 ; times++) + { + PID_PostionalPID(target,actual1,&pid1); + PID_IncrementalPID(target,actual2,&pid2); + Limit_i(&pid1); + Limit_i(&pid2); + printf("E=%2f E=%2f\n",pid1.err,pid2.err); + } + + return 0; +} \ No newline at end of file diff --git a/C_PID/Doge-ai/try/pid.c b/C_PID/Doge-ai/try/pid.c new file mode 100644 index 0000000..8d7e4d0 --- /dev/null +++ b/C_PID/Doge-ai/try/pid.c @@ -0,0 +1,63 @@ +#include +#include"pid.h" + +int PID_Init(PID*pid) +{ + pid->kp=0.0; + pid->ki=0.0; + pid->kd=0.0; + pid->err=0.0; + pid->target=100; + pid->errLast=0.0; + pid->errSum=0.0; + pid->out=0.0; + pid->LastOut=0.0; + pid->inte=0.0; + pid->actual=0.0; + return 0; +} +int Limit_i(PID *pid) +{ + if (pid->inte > 100); + pid->inte = 100 ; + if (pid->inte <= 100); + pid->inte = 100; + return 0;//对i设限 +} +int PID_SetParameter(float kp, float ki, float kd, PID*pid) +{ + pid->kp=kp; + pid->ki=ki; + pid->kd=kd; + return 0; +} +int PID_PostionalPID(float target, float actual, PID*pid) +{ + pid->target=target; + pid->actual=actual; + + pid->err=pid->target-pid->actual; + pid->inte+=pid->err; + pid->errSum+=pid->err; + + pid->inte+=pid->err; + Limit_i(pid); + pid->out=pid->kp*pid->err+pid->ki*pid->inte+pid->kd*(pid->err-pid->errLast); + pid->errLast=pid->err; + + return pid->out; +} +int PID_IncrementalPID(float target, float actual, PID*pid) +{ + pid->target=target; + pid->actual=actual; + pid->err=pid->target-pid->actual; + + pid->out=pid->LastOut+pid->kp*(pid->err-pid->errLast)+pid->ki*pid->err+pid->kd*(pid->err-2*pid->errLast+pid->errSum); + + pid->errLast=pid->err; + pid->errSum+=pid->err; + pid->LastOut=pid->out; + + return pid->out; +} diff --git a/C_PID/Doge-ai/try/pid.h b/C_PID/Doge-ai/try/pid.h new file mode 100644 index 0000000..ecb5ee8 --- /dev/null +++ b/C_PID/Doge-ai/try/pid.h @@ -0,0 +1,24 @@ +#pragma once +#ifndef _PID_H_ +#define _PID_H_ + +typedef struct +{ + float LastOut,out;//输出 + float inte;//积分 + float kp,ki,kd;//比例积分微分系数 + float target;//目标值 + float actual;//实际值 + float errSum;//误差和 + float err;//误差s + float errLast;//上次误差 + + +} PID; + + int PID_Init(PID*pid); //初始化PID结构体 + int PID_SetParameter(float kp, float ki, float kd, PID*pid); //设置PID参数 + int PID_PostionalPID(float target, float actual, PID*pid); //位置式PID实现 + int PID_IncrementalPID(float target, float actual, PID*pid); //增量式PID实现 + + #endif \ No newline at end of file