/*==============================================================================
此程序是自己在做速度PID控制时写的 借鉴了一些资料 希望对大家有点帮助 不足之处还望指教
PID模块相关函数库
编译环境:IAR 4.30
作者: L_jeary SEU
日期: 2010-8-4
说明: PID的增量式和位置式算法的实现
由于舵机的转向由固定的PWM占空比决定,应考虑位置式PID算法
由于驱动电动机的电压会随着电池电压的降低而降低,应考虑增量式PID算法
==============================================================================*/
#ifndef __PID_h
#define __PID_h
//PID相关参数宏定义
#define Err_max 45//阀值上限
#define Err_min -45//阀值下限
//右侧电机PID参数
#define R_PID_Kp 1.2//比例常数
#define R_PID_Ki 0.7//积分常数
#define R_PID_Kd 0.5//微分常数
//左侧电机PID参数
#define L_PID_Kp 1.2//比例常数
#define L_PID_Ki 0.7//积分常数
#define L_PID_Kd 0.5//微分常数
//函数声明
void PID_Init(void);
//增量式PID函数
int RM_IncPIDCalc(int NextPoint);
int LM_IncPIDCalc(int NextPoint);
//位置式PID函数
unsigned int RM_LocPIDCalc(int NextPoint);
unsigned int LM_LocPIDCalc(int NextPoint);
//PID 相关参数结构体
typedef struct PID
{
int SetPoint; //给定值 Desired Value
long SumError; //误差累计
float Proportion; //比例常数 Proportional Const
float Integral; //积分常数 Integral Const
float Derivative; //微分常数 Derivative Const
int LastError; //Error[-1]
int PrevError; //Error[-2]
} PID;
static PID RM_sPID;//定义右侧电机PID参数结构体
static PID *RM_sptr = &RM_sPID;
static PID LM_sPID;//定义左侧电机PID参数结构体
static PID *LM_sptr = &LM_sPID;
/*==============================================================================
程序名称:PID_Init()
程序功能:PID参数初始化函数
注意事项:
提示说明:
输 入:
返 回:
==============================================================================*/
void PID_Init(void)
{
//右侧电机PID参数初始化
RM_sptr->SumError = 0;
RM_sptr->LastError = 0; //Error[-1]
RM_sptr->PrevError = 0; //Error[-2]
RM_sptr->Proportion = R_PID_Kp; //比例常数 Proportional Const
RM_sptr->Integral = R_PID_Ki; //积分常数Integral Const
RM_sptr->Derivative = R_PID_Kd; //微分常数 Derivative Const
RM_sptr->SetPoint = 0;
//左侧电机PID参数初始化
LM_sptr->SumError = 0;
LM_sptr->LastError = 0; //Error[-1]
LM_sptr->PrevError = 0; //Error[-2]
LM_sptr->Proportion = L_PID_Kp; //比例常数 Proportional Const
LM_sptr->Integral = L_PID_Ki; //积分常数Integral Const
LM_sptr->Derivative = L_PID_Kd; //微分常数 Derivative Const
LM_sptr->SetPoint = 0;
}
/*==============================================================================
程序名称:IncPIDCalc(int NextPoint)
程序功能:增量式PID控制函数
注意事项:该函数只是一个范例,具体应用时应加以改进
提示说明:增量式PID是指数字控制器的输出只是控制量的增量Δuk。
当执行机构需要的控制量是增量,而不是位置量的绝对数值时,
可以使用增量式PID控制算法进行控制。
输 入:NextPoint 当前输出值
返 回:iIncpid 增量式PID控制器输出值
==============================================================================*/
/*int IncPIDCalc(int NextPoint)
{
int iError, iIncpid;
//当前误差
iError = sptr->SetPoint - NextPoint;
//增量计算
iIncpid = sptr->Proportion * iError //E[k]项
- sptr->Integral * sptr->LastError //E[k-1]项
+ sptr->Derivative * sptr->PrevError; //E[k-2]项
//存储误差,用于下次计算
sptr->PrevError = sptr->LastError;
sptr->LastError = iError;
//返回增量值
return iIncpid;
}*/
/*==============================================================================
程序名称:RM_IncPIDCalc(int NextPoint)
程序功能:右侧电机增量式PID控制函数
注意事项:
提示说明:
输 入:NextPoint 当前输出值
返 回:iIncpid 增量式PID控制器 输出的增量值
==============================================================================*/
int RM_IncPIDCalc(int NextPoint)
{
int iError, iIncpid;
//当前误差
iError = RM_sptr->SetPoint - NextPoint;
//增加阀值过滤功能
if(iError>Err_max)//判断是否超过上限
{
iError=Err_max;
}
if(iError<Err_min)//判断是否超过下限
{
iError=Err_min;
}
//增量计算
iIncpid = RM_sptr->Proportion * iError //E[k]项
- RM_sptr->Integral * RM_sptr->LastError //E[k-1]项
+ RM_sptr->Derivative * RM_sptr->PrevError; //E[k-2]项
//存储误差,用于下次计算
RM_sptr->PrevError = RM_sptr->LastError;
RM_sptr->LastError = iError;
//返回增量值
return iIncpid;
}
/*==============================================================================
程序名称:LM_IncPIDCalc(int NextPoint)
程序功能:左侧电机增量式PID控制函数
注意事项:
提示说明:
输 入:NextPoint 当前输出值
返 回:iIncpid 增量式PID控制器 输出的增量值
==============================================================================*/
int LM_IncPIDCalc(int NextPoint)
{
int iError, iIncpid;
//当前误差
iError = LM_sptr->SetPoint - NextPoint;
//增加阀值过滤功能
if(iError>Err_max)//判断是否超过上限
{
iError=Err_max;
}
if(iError<Err_min)//判断是否超过下限
{
iError=Err_min;
}
//增量计算
iIncpid = LM_sptr->Proportion * iError //E[k]项
- LM_sptr->Integral * LM_sptr->LastError //E[k-1]项
+ LM_sptr->Derivative * LM_sptr->PrevError; //E[k-2]项
//存储误差,用于下次计算
LM_sptr->PrevError = LM_sptr->LastError;
LM_sptr->LastError = iError;
//返回增量值
return iIncpid;
}
/*==============================================================================
程序名称:LocPIDCalc(int NextPoint)
程序功能:位置式PID控制函数
注意事项:该函数只是一个范例,具体应用时应加以改进
提示说明:由于全量输出,所以每次输出均与过去状态有关,计算时要对ek进行累加,
工作量大,并且因为计算机输出的对应的是执行机构的实际位置,
如果计算机出现故障,输出的将大幅度变化,会引起执行机构的大幅度变化,
有可能因此造成严重的生产事故,这在实生产际中是不允许的。
输 入:NextPoint 当前输出值
返 回:iLocpid 位置式PID控制器输出值
==============================================================================*/
/*unsigned int LocPIDCalc(int NextPoint)
{
int iError,dError;
int iLocpid;
iError = sptr->SetPoint - NextPoint; //偏差
sptr->SumError += iError; //积分
dError = iError - sptr->LastError; //微分
sptr->LastError = iError;
iLocpid=sptr->Proportion * iError//比例项
+ sptr->Integral * sptr->SumError//积分项
+ sptr->Derivative * dError;//微分项
return iLocpid;
}*/
/*==============================================================================
程序名称:RM_LocPIDCalc(int NextPoint)
程序功能:右侧电机位置式PID控制函数
注意事项:
提示说明:
输 入:NextPoint 当前输出值
返 回:iLocpid 位置式PID控制器输出值
==============================================================================*/
unsigned int RM_LocPIDCalc(int NextPoint)
{
int iError,dError;
int iLocpid;
iError = RM_sptr->SetPoint - NextPoint; //偏差
//增加阀值过滤功能
if(iError>Err_max)//判断是否超过上限
{
iError=Err_max;
}
if(iError<Err_min)//判断是否超过下限
{
iError=Err_min;
}
RM_sptr->SumError += iError; //积分
dError = iError - RM_sptr->LastError; //微分
RM_sptr->LastError = iError;
iLocpid=RM_sptr->Proportion * iError//比例项
+ RM_sptr->Integral * RM_sptr->SumError//积分项
+ RM_sptr->Derivative * dError;//微分项
if(iLocpid<0)//确保控制器输出的值为正
{
iLocpid=0;
}
return (unsigned int)iLocpid;
}
/*==============================================================================
程序名称:LM_LocPIDCalc(int NextPoint)
程序功能:左侧电机位置式PID控制函数
注意事项:
提示说明:
输 入:NextPoint 当前输出值
返 回:iLocpid 位置式PID控制器输出值
==============================================================================*/
unsigned int LM_LocPIDCalc(int NextPoint)
{
int iError,dError;
int iLocpid;
iError = LM_sptr->SetPoint - NextPoint; //偏差
//增加阀值过滤功能
if(iError>Err_max)//判断是否超过上限
{
iError=Err_max;
}
if(iError<Err_min)//判断是否超过下限
{
iError=Err_min;
}
LM_sptr->SumError += iError; //积分
dError = iError - LM_sptr->LastError; //微分
LM_sptr->LastError = iError;
iLocpid=LM_sptr->Proportion * iError//比例项
+ LM_sptr->Integral * LM_sptr->SumError//积分项
+ LM_sptr->Derivative * dError;//微分项
if(iLocpid<0)//确保控制器输出的值为正
{
iLocpid=0;
}
return (unsigned int)iLocpid;
}
#endif
评论