位置式PID精简代码,电机驱动 C/C++

admin 1月前 187

位置式PID精简代码

/**************************************/  
//        位置式PID算法  
/**************************************/  
//float SET;//设定值  
U32   KP=2;//比例常数  
U32   KI=0;//积分常数  
U32   KD=0;//微分常数  
U32   LE;//上次误差值  
U32   SE;//累计误差值  
  
U32 PID_Ctl(U32 SET,U32 Samp_Data)  
{  
	S32 new,E,DE,out;//定义有符号临时变量
	
    new=Samp_Data;//读取采样值  
    E=SET-new;//计算当前偏差值  
    SE+=E;//计算误差积分值  
    DE=E-LE;//计算当前误差差分值 
    out=KP*E+KI*SE+KD*DE;//计算控制信号 
    
    out=out+2048;//0-2048反转,2048-4095正转
    
    if(out<=0)//输出电压0没有意义
    out=0;
    if(out>=4095)//输出电压大于4095无意义
    out=4095; 
    
    Write_DA(out);//计算输出结果    
    LE=E;//保存误差值,以供一次计算使用  
  
    return out;//返回当前采样值,即实际输出值
}


最新回复 [0]
返回