下面的是头文件
#ifndef _PID_H #define _PID_H typedef struct _positional_pid{ //PID的基本参数 double GoalVale; //目标值 double ActualVale; //真实值 double Error; //误差 double LastError; //上一次的误差 double Kp,Ki,Kd; //PID三参数 double T; //周期 double Output; //输出 double Integral; //积分值 //状态参数 int Flag_First; //首次运算的标志 double MaxOutput; //输出限幅 double MinOutput; //输出限幅 double IntePartValue; //积分分离阈值 double MAXInte; //积分限幅 double NoneActValue; //不动作区阈值 }PositionalPid; typedef struct _incremental_pid{ //PID的基本参数 double GoalVale; //目标值 double ActualVale; //真实值 double Error; //误差 double LastError1; //上一次的误差 double LastError2; //上上次的误差 double Kp,Ki,Kd; //PID三参数 double T; //周期 double Output; //输出 //状态参数 int Flag_First; //首次运算的标志 double MaxOutput; //输出限幅 double MinOutput; //输出限幅 double NoneActValue; //不动作区阈值 }IncrementalPid; //位置式PID计算公式 void positionalPid_Cal(PositionalPid *pid , double ActualValue); //增量式PID计算公式 void incrementalPid_Cal(IncrementalPid *pid , double ActualValue); #endif
下面的就是源文件,有位置式PID和增量式PID
#include "pid.h" #includevoid positionalPid_Cal(PositionalPid *pid , double ActualValue){ pid->ActualVale = ActualValue; pid->Error = pid->GoalVale - pid->ActualVale; //设定值-目前值 if(fabs(pid->Error) < pid->NoneActValue){ //死区 pid->Integral=0; return ; } double p_temp = pid->Kp * pid->Error; if(fabs(pid->Error) > pid->IntePartValue){ pid->Integral=0; }else{ if(fabs(pid->Integral)>pid->MAXInte) pid->Integral=pid->Integral>0?pid->MAXInte:(-pid->MAXInte); else pid->Integral += pid->Error; } double i_temp=pid->Integral * pid->Ki; double d_temp; if(pid->Flag_First){ d_temp=pid->Kd*(pid->Error-pid->LastError); pid->LastError=pid->Error; }else{ pid->Flag_First=1; d_temp=0; pid->LastError=pid->Error; } pid->Output=p_temp+i_temp+d_temp; if(pid->Output < pid->MinOutput) pid->Output = pid->MinOutput; if(pid->Output > pid->MaxOutput) pid->Output = pid->MaxOutput; pid->LastError = pid->Error; } void incrementalPid_Cal(IncrementalPid *pid , double ActualValue){ pid->ActualVale = ActualValue; pid->Error = pid->GoalVale - pid->ActualVale; //设定值-目前值 if(fabs(pid->Error) < pid->NoneActValue){ //死区 pid->Output=0; return ; } if(pid->Flag_First>=2){ pid->Output = pid->Kp * pid->Error - pid->Ki*pid->LastError1 + pid->Kd * pid->LastError2; if(pid->Output < pid->MinOutput) pid->Output = pid->MinOutput; if(pid->Output > pid->MaxOutput) pid->Output = pid->MaxOutput; }else{ pid->Flag_First++; } pid->LastError2 = pid->LastError1; pid->LastError1 = pid->Error; }
今天的就这样啦,写作业去了。
欢迎分享,转载请注明来源:内存溢出
评论列表(0条)