增量式:
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;
}
相关截图参考以下链接: