PID速度控制函数 可用

void speed_control(void)
{

float a,b;
a=abs(mid_err1);
if(stopflag==1) //stopflag是起跑线标志
{
speed_mid=0;
}
else
{
if(a<6)
{
speed_mid=zhixianspeed_mid;
}
else if(a<20)
{
speed_mid=ruwanspeed_mid1;
}
else
{
speed_mid=ruwanspeed_mid1-10;
}

}
/* if(stop==1)
{
speed_mid=0;
fanzhuanduty=1000;
if(ch0_pulseacc<3)
{
fanzhuanduty=0;
}
}*/
speed_pluse=ch0_pulseacc;//speed_pluse=pluse/(float)speedcount;
if(speed_pluse<5) //当编码器的值很小的时候，让车停下来
{
stop=1;
}
speed_err1=speed_mid-speed_pluse;
speedoutold=speedoutnew;
p_value=K_p*(speed_err1-speed_err2);
i_value=K_i*speed_err1;
d_value=K_d*(speed_err1-2*speed_err2+speed_err3);
speedoutnew=speedoutold+(p_value+i_value+d_value);//著名的pid算法，绝对正确
speed_err3=speed_err2;
speed_err2=speed_err1;
if(stop==1)
{
speedoutnew=0;
}
if(speedoutnew<-6000)
{
speedoutnew=-6000;
}
if(speedoutnew>6000)
{
speedoutnew=6000;
}
if((speed_pluse>speed_mid)&&(a<10)&&(stopflag==0)) //刚入弯的时
{ //候，如果当前速度大于目标速度，清
LPLD_FTM_PWM_ChangeDuty(FTM1,FTM_Ch0,0); //零
speed_zhen;
speedoutnew=0;
speed_err1=0;
speed_err2=0;
}
else
{
if(speedoutnew>0)
{
LPLD_FTM_PWM_ChangeDuty(FTM1,FTM_Ch0,(uint32)speedoutnew);
speed_zhen;
}
else
{
b=-speedoutnew;
LPLD_FTM_PWM_ChangeDuty(FTM1,FTM_Ch0,(uint32)b);//正转方向占空比为0 反转
speed_fang;
}
}

}