同步步兵代码，暂时关闭底盘跟随
This commit is contained in:
202
云台/云台-old/Control/PID.c
Normal file
202
云台/云台-old/Control/PID.c
Normal file
@@ -0,0 +1,202 @@
|
||||
#include "stm32f4xx.h" // Device header
|
||||
#include "stm32f4xx_conf.h"
|
||||
#include "PID.h"
|
||||
|
||||
/*
|
||||
*函数简介:位置式PID初始化结构体
|
||||
*参数说明:位置式PID参数结构体
|
||||
*参数说明:预期值
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void PID_PositionStructureInit(PID_PositionInitTypedef* PID_InitStructure,float NeedValue)
|
||||
{
|
||||
PID_InitStructure->Need_Value=NeedValue;
|
||||
PID_InitStructure->Ek=0;
|
||||
PID_InitStructure->Sum_Ek=0;
|
||||
PID_InitStructure->Ek_low=0;
|
||||
PID_InitStructure->Ek_up=0;
|
||||
PID_InitStructure->Kp=0;
|
||||
PID_InitStructure->Ki=0;
|
||||
PID_InitStructure->Kd=0;
|
||||
PID_InitStructure->OUT_low=-1e10;
|
||||
PID_InitStructure->OUT_up=1e10;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:位置式PID设置参数
|
||||
*参数说明:位置式PID参数结构体
|
||||
*参数说明:单精度浮点型Kp
|
||||
*参数说明:单精度浮点型Ki
|
||||
*参数说明:单精度浮点型Kd
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void PID_PositionSetParameter(PID_PositionInitTypedef* PID_InitStructure,float kp,float ki,float kd)
|
||||
{
|
||||
PID_InitStructure->Kp=kp;
|
||||
PID_InitStructure->Ki=ki;
|
||||
PID_InitStructure->Kd=kd;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:位置式PID设置误差为0阈值
|
||||
*参数说明:位置式PID参数结构体
|
||||
*参数说明:误差为0阈值下限
|
||||
*参数说明:误差为0阈值上限
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void PID_PositionSetEkRange(PID_PositionInitTypedef* PID_InitStructure,float ek_low,float ek_up)
|
||||
{
|
||||
PID_InitStructure->Ek_low=ek_low;
|
||||
PID_InitStructure->Ek_up=ek_up;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:位置式PID设置输出限幅
|
||||
*参数说明:位置式PID参数结构体
|
||||
*参数说明:输出限幅下限
|
||||
*参数说明:输出限幅上限
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void PID_PositionSetOUTRange(PID_PositionInitTypedef* PID_InitStructure,float out_low,float out_up)
|
||||
{
|
||||
PID_InitStructure->OUT_low=out_low;
|
||||
PID_InitStructure->OUT_up=out_up;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:位置式PID清理
|
||||
*参数说明:位置式PID参数结构体
|
||||
*返回类型:无
|
||||
*备注:使Ek和Sum为0
|
||||
*/
|
||||
void PID_PositionClean(PID_PositionInitTypedef* PID_InitStructure)
|
||||
{
|
||||
PID_InitStructure->Ek=0;
|
||||
PID_InitStructure->Sum_Ek=0;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:位置式PID计算
|
||||
*参数说明:位置式PID参数结构体
|
||||
*参数说明:当前值
|
||||
*返回类型:无
|
||||
*备注:OUT=POUT+IOUT+DOUT=Kp*Ek+Ki*ΣEk+Kd*(Ek-Ek_1)
|
||||
*备注:计算结果保存在位置式PID参数结构体中
|
||||
*/
|
||||
void PID_PositionCalc(PID_PositionInitTypedef* PID_InitStructure,float NowValue)
|
||||
{
|
||||
PID_InitStructure->Now_Value=NowValue;
|
||||
PID_InitStructure->Ek_1=PID_InitStructure->Ek;
|
||||
PID_InitStructure->Ek=PID_InitStructure->Need_Value-PID_InitStructure->Now_Value;
|
||||
if(PID_InitStructure->Ek_low<PID_InitStructure->Ek&&PID_InitStructure->Ek<PID_InitStructure->Ek_up)//误差为0检测
|
||||
PID_InitStructure->Ek=0;
|
||||
PID_InitStructure->Sum_Ek+=PID_InitStructure->Ek;
|
||||
PID_InitStructure->Del_Ek=PID_InitStructure->Ek-PID_InitStructure->Ek_1;
|
||||
|
||||
PID_InitStructure->P_OUT=PID_InitStructure->Kp*PID_InitStructure->Ek;
|
||||
PID_InitStructure->I_OUT=PID_InitStructure->Ki*PID_InitStructure->Sum_Ek;
|
||||
PID_InitStructure->D_OUT=PID_InitStructure->Kd*PID_InitStructure->Del_Ek;
|
||||
PID_InitStructure->OUT=PID_InitStructure->P_OUT+PID_InitStructure->I_OUT+PID_InitStructure->D_OUT;
|
||||
|
||||
if(PID_InitStructure->OUT<PID_InitStructure->OUT_low)//输出限幅
|
||||
PID_InitStructure->OUT=PID_InitStructure->OUT_low;
|
||||
if(PID_InitStructure->OUT>PID_InitStructure->OUT_up)
|
||||
PID_InitStructure->OUT=PID_InitStructure->OUT_up;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:增量式PID初始化结构体
|
||||
*参数说明:增量式PID参数结构体
|
||||
*参数说明:预期值
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void PID_IncrementalStructureInit(PID_IncrementalInitTypedef* PID_InitStructure,float NeedValue)
|
||||
{
|
||||
PID_InitStructure->Need_Value=NeedValue;
|
||||
PID_InitStructure->Ek=0;
|
||||
PID_InitStructure->Ek_1=0;
|
||||
PID_InitStructure->Ek_low=0;
|
||||
PID_InitStructure->Ek_up=0;
|
||||
PID_InitStructure->Kp=0;
|
||||
PID_InitStructure->Ki=0;
|
||||
PID_InitStructure->Kd=0;
|
||||
PID_InitStructure->OUT_low=-1e10;
|
||||
PID_InitStructure->OUT_up=1e10;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:增量式PID设置参数
|
||||
*参数说明:增量式PID参数结构体
|
||||
*参数说明:单精度浮点型Kp
|
||||
*参数说明:单精度浮点型Ki
|
||||
*参数说明:单精度浮点型Kd
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void PID_IncrementalSetParameter(PID_IncrementalInitTypedef* PID_InitStructure,float kp,float ki,float kd)
|
||||
{
|
||||
PID_InitStructure->Kp=kp;
|
||||
PID_InitStructure->Ki=ki;
|
||||
PID_InitStructure->Kd=kd;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:增量式PID设置误差为0阈值
|
||||
*参数说明:增量式PID参数结构体
|
||||
*参数说明:误差为0阈值下限
|
||||
*参数说明:误差为0阈值上限
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void PID_IncrementalSetEkRange(PID_IncrementalInitTypedef* PID_InitStructure,float ek_low,float ek_up)
|
||||
{
|
||||
PID_InitStructure->Ek_low=ek_low;
|
||||
PID_InitStructure->Ek_up=ek_up;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:增量式PID设置输出限幅
|
||||
*参数说明:增量式PID参数结构体
|
||||
*参数说明:输出限幅下限
|
||||
*参数说明:输出限幅上限
|
||||
*返回类型:无
|
||||
*备注:无
|
||||
*/
|
||||
void PID_IncrementalSetOUTRange(PID_IncrementalInitTypedef* PID_InitStructure,float out_low,float out_up)
|
||||
{
|
||||
PID_InitStructure->OUT_low=out_low;
|
||||
PID_InitStructure->OUT_up=out_up;
|
||||
}
|
||||
|
||||
/*
|
||||
*函数简介:增量式PID计算
|
||||
*参数说明:增量式PID参数结构体
|
||||
*参数说明:当前值
|
||||
*返回类型:无
|
||||
*备注:OUT=POUT+IOUT+DOUT=Kp*ΔEk+Ki*ΣΔEk+Kd*(ΔEk-ΔEk_1)=Kp*(Ek-Ek_1)+Ki*Ek+Kd*(Ek-2*Ek_1+Ek_2)
|
||||
*备注:计算结果保存在增量式PID参数结构体中
|
||||
*/
|
||||
void PID_IncrementalCalc(PID_IncrementalInitTypedef* PID_InitStructure,float NowValue)
|
||||
{
|
||||
PID_InitStructure->Now_Value=NowValue;
|
||||
PID_InitStructure->Ek_2=PID_InitStructure->Ek_1;
|
||||
PID_InitStructure->Ek_1=PID_InitStructure->Ek;
|
||||
PID_InitStructure->Ek=PID_InitStructure->Need_Value-PID_InitStructure->Now_Value;
|
||||
if(PID_InitStructure->Ek_low<PID_InitStructure->Ek&&PID_InitStructure->Ek<PID_InitStructure->Ek_up)//误差为0检测
|
||||
PID_InitStructure->Ek=0;
|
||||
|
||||
PID_InitStructure->P_OUT=PID_InitStructure->Kp*(PID_InitStructure->Ek-PID_InitStructure->Ek_1);
|
||||
PID_InitStructure->I_OUT=PID_InitStructure->Ki*PID_InitStructure->Ek;
|
||||
PID_InitStructure->D_OUT=PID_InitStructure->Kd*(PID_InitStructure->Ek-2*PID_InitStructure->Ek_1+PID_InitStructure->Ek_2);
|
||||
PID_InitStructure->OUT=PID_InitStructure->P_OUT+PID_InitStructure->I_OUT+PID_InitStructure->D_OUT;
|
||||
|
||||
if(PID_InitStructure->OUT<PID_InitStructure->OUT_low)//输出限幅
|
||||
PID_InitStructure->OUT=PID_InitStructure->OUT_low;
|
||||
if(PID_InitStructure->OUT>PID_InitStructure->OUT_up)
|
||||
PID_InitStructure->OUT=PID_InitStructure->OUT_up;
|
||||
}
|
||||
63
云台/云台-old/Control/PID.h
Normal file
63
云台/云台-old/Control/PID.h
Normal file
@@ -0,0 +1,63 @@
|
||||
#ifndef __PID_H__
|
||||
#define __PID_H__
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float Need_Value;//预期值
|
||||
float Now_Value;//当前值
|
||||
|
||||
float Ek;//本次误差
|
||||
float Ek_1;//上一次误差
|
||||
float Sum_Ek;//误差积分
|
||||
float Del_Ek;//误差差分
|
||||
float Ek_low;//误差为0阈值下限
|
||||
float Ek_up;//误差为0阈值上限
|
||||
|
||||
float Kp;//Kp
|
||||
float Ki;//Ki
|
||||
float Kd;//Kd
|
||||
|
||||
float P_OUT;//比例输出
|
||||
float I_OUT;//积分输出
|
||||
float D_OUT;//微分输出
|
||||
float OUT;//总输出
|
||||
float OUT_low;//输出限幅下限
|
||||
float OUT_up;//输出限幅上限
|
||||
}PID_PositionInitTypedef;//位置式PID参数结构体
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float Need_Value;//预期值
|
||||
float Now_Value;//当前值
|
||||
|
||||
float Ek;//本次误差
|
||||
float Ek_1;//上一次误差
|
||||
float Ek_2;//上两次误差
|
||||
float Ek_low;//误差为0阈值下限
|
||||
float Ek_up;//误差为0阈值上限
|
||||
|
||||
float Kp;//Kp
|
||||
float Ki;//Ki
|
||||
float Kd;//Kd
|
||||
|
||||
float P_OUT;//比例输出
|
||||
float I_OUT;//积分输出
|
||||
float D_OUT;//微分输出
|
||||
float OUT;//总输出
|
||||
float OUT_low;//输出限幅下限
|
||||
float OUT_up;//输出限幅上限
|
||||
}PID_IncrementalInitTypedef;//增量式PID参数结构体
|
||||
|
||||
void PID_PositionStructureInit(PID_PositionInitTypedef* PID_InitStructure,float NeedValue);//位置式PID初始化结构体
|
||||
void PID_PositionSetParameter(PID_PositionInitTypedef* PID_InitStructure,float kp,float ki,float kd);//位置式PID设置参数
|
||||
void PID_PositionSetEkRange(PID_PositionInitTypedef* PID_InitStructure,float ek_low,float ek_up);//位置式PID设置误差为0阈值
|
||||
void PID_PositionSetOUTRange(PID_PositionInitTypedef* PID_InitStructure,float out_low,float out_up);//位置式PID设置输出限幅
|
||||
void PID_PositionClean(PID_PositionInitTypedef* PID_InitStructure);//位置式PID清理
|
||||
void PID_PositionCalc(PID_PositionInitTypedef* PID_InitStructure,float NowValue);//位置式PID计算
|
||||
void PID_IncrementalStructureInit(PID_IncrementalInitTypedef* PID_InitStructure,float NeedValue);//增量式PID初始化结构体
|
||||
void PID_IncrementalSetParameter(PID_IncrementalInitTypedef* PID_InitStructure,float kp,float ki,float kd);//增量式PID设置参数
|
||||
void PID_IncrementalSetEkRange(PID_IncrementalInitTypedef* PID_InitStructure,float ek_low,float ek_up);//增量式PID设置误差为0阈值
|
||||
void PID_IncrementalSetOUTRange(PID_IncrementalInitTypedef* PID_InitStructure,float out_low,float out_up);//增量式PID设置输出限幅
|
||||
void PID_IncrementalCalc(PID_IncrementalInitTypedef* PID_InitStructure,float NowValue);//增量式PID计算
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user