增量PID与位置式PID

唔好理总之好犀利

已于 2024-08-06 11:00:35 修改

阅读量167

点赞数 1

文章标签：嵌入式软件 stm32 GD32 PID控制

于 2024-08-06 10:59:56 首次发布

本文链接：https://blog.csdn.net/weixin_39257775/article/details/140949741

版权

增量式：

void calculatePwmL(u8 cmd)
{
	s16 aimSpeedL=0, cur_err = 0;
	static s16 last_err = 0,form_err = 0;
	static s16 pwmL=0;
	static u8 stopFlag=0;

	if(cmd != 1 && !(IS_TIM25MS))
	{
		return;
	}
 
	aimSpeedL = motionDat.speedL*10;
	cur_err=aimSpeedL - curSpeedL;
	if(motionDat.mode == 1)
	{
		pwmL+=(3*Kp*(cur_err - last_err) + 3*Ki*cur_err + 3*Kd*(cur_err-2*last_err+form_err))/20;
	}
	else
	{
		pwmL+=(Kp*(cur_err - last_err) + Ki*cur_err + Kd*(cur_err-2*last_err+form_err))/20;
	}
	if(aimSpeedL==0&&abs(curSpeedL)< 10){
		pwmL=0;
		if(curSpeedL<5){
			stopFlag=1;
		}
	}
	if(aimSpeedL!=0){
	stopFlag = 0;
	}
	if(stopFlag){
		pwmL=0;
	}
	pwmL=CONSTRAIN(pwmL, -1200, 1200);
	
	form_err=last_err;
	last_err=cur_err;
	if(pwmL<0){
		WHEEL_L_ON()
		WHEEL_L_CCW(abs(pwmL));
	}else if(pwmL > 0){
		WHEEL_L_ON()
		WHEEL_L_CW(pwmL);
	}else{
		WHEEL_L_OFF()
		WHEEL_L_CW(0);
	}
}

位置式：

int16_t sSpeed_R_PID(int16_t RefV,int16_t FdbV)
{
    int16_t  UpV=0;
    int16_t  ErrV,OutTempV;
    int16_t  OutputV;

    ErrV = RefV - FdbV;//误差速度

    UpV = P_V*ErrV;      // 计算出比例部分
    
    SpeedRightUiV = SpeedRightUiV + ((float)(UpV*I_V));//计算出积分累加部分

    
    /*积分范围限制，不能无限累加*/
    if(SpeedRightUiV>UIMAXV)
    {
        SpeedRightUiV=UIMAXV;
    }else
    if(SpeedRightUiV<UIMINV)
    {
        SpeedRightUiV=UIMINV;
    }
    
    OutTempV = UpV+(int16_t)SpeedRightUiV;  //PI

    /*输出速度范围限制*/
    if (OutTempV > OUTMAXV)
    {
        OutputV =  OUTMAXV;
    }
    else if (OutTempV < OUTMINV)
    {
        OutputV = OUTMINV;
    }
    else
    {
        OutputV = OutTempV;
    }

    if(RefV>=0)
    {
        if(OutputV<0) OutputV=1;
    }
    else if(RefV<0)
    {
        if(OutputV>0) OutputV=-1;
    }
    return OutputV;
}