//算法调用方式
PID_InitTypeDef pid1;
pid1.enable=1;
pid1.enable_lim_ouput=1;
pid1.enable_lim_sum_error=1;
pid1.lim_sum_error=2000.0;
pid1.lim_up_output=180.0;
pid1.lim_down_output=0.0;
pid1.kp=1.5;
pid1.ki=0.01;
pid1.kd=0.01;
out=pidProcess(&pid1,90, deg);
motor_driver(out);
//结构体定义
typedef struct
{
uint8_t enable; //输出使能
uint8_t enable_lim_sum_error; //积分限幅使能
uint8_t enable_lim_ouput; //输出限幅使能
double kp; //比例参数
double ki; //积分参数
double kd; //微分参数
double lim_sum_error; //误差积分限幅
double lim_up_output; //输出限幅 防止输出过大
double lim_down_output;
double sum_error; //误差积分
double last_error; //上一次的误差
double last_last_error; //上上一次的误差
double kd_output;
double ki_output;
double kp_output;
double error_dec;
double pid_output;
double error;
}PID_InitTypeDef;
//代码运算部分 //定义结构体 设定值 实际值
double PidProcess(PID_InitTypeDef* pid,double input, double measure)
{
double output = 0;
double error = input - measure; //计算误差
pid->sum_error += error;
//误差积分限幅
if(pid->enable_lim_sum_error == 1 && fabs(pid->sum_error) > pid->lim_sum_error)
{
if(pid->sum_error > 0)
pid->sum_error = pid->lim_sum_error;
else
pid->sum_error = -pid->lim_sum_error;
}
output = pid->kp * error
+ pid->ki * pid->sum_error
+ pid->kd * (error - pid->last_error) ;
/**************************************************************/
//用于观察参数，可不加
pid->kp_output = pid->kp * error;
pid->ki_output = pid->ki * pid->sum_error;
pid->kd_output = pid->kd * (error - pid->last_error);
pid->error_dec = error - pid->last_error;
/*************************************************************/
//输出限幅
if(pid->enable_lim_ouput == 1)
{
if(output > pid->lim_up_output)
output = pid->lim_up_output;
if(output < pid->lim_down_output)
output = pid->lim_down_output;
}
//更新误差值
pid->last_error = error;
//是否使能输出
if(pid->enable == 1)
{
pid->pid_output = output;
return output;
}
else
{
pid->pid_output = 0;
return 0;
}
}
|
楼主
标题置顶
标题高亮
