博客

WHEELTEC的串级pid参考代码

float Position_KP=0.5,Position_KI=0.001,Position_KD=0.5; //PID系数
float Velocity_KP=150,Velocity_KI=20,Velocity_KD=150; //PID系数
int Encoder,Target_Velocity=20;
int Moto,Position_Moto;//电机PWM变量 应是Motor的 向Moto致敬
int limit_a;
int Position,Target_Position=10000; //位置和目标位置自己设定

/*********************************************************************
函数功能：位置式PID控制器
入口参数：编码器测量位置信息，目标位置
返回  值：电机PWM
根据位置式离散PID公式 
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  
∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,,k;
pwm代表输出
********************************************************************/
int Position_PID (int position,int target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=target-position;                                
	 Integral_bias+=Bias;	                            
	 Integral_bias=Xianfu(Integral_bias,myabs(Target_Velocity));
	 Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);
	 Last_Bias=Bias;                                      
	
	if(Pwm>10000)Pwm=10000;
	if(Pwm<-10000)Pwm=-10000;
	return Pwm;                                           
}


/**************************************************************
函数功能：增量PI控制器
入口参数：编码器测量值，目标速度
返回  值：电机PWM
根据增量式离散PID公式 
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  以此类推 
pwm代表增量输出
在我们的速度控制闭环系统里面，只使用PI控制
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
//增量式的pi控制应该如上所示
****************************************************************/
int Incremental_PI (int Encoder,int Target)
{ 	
	 static float Bias,Pwm,Last_bias,Integral_bias;
	 Bias=Target-Encoder;//计算偏差
	 Integral_bias+=Bias;
	 Integral_bias=Xianfu(Integral_bias,5000);
	 Pwm=Velocity_KP*Bias+
		 Velocity_KI*Integral_bias+
		 Velocity_KD*(Bias-Last_bias);   //增量式PI控制器
	 Last_bias=Bias;	                                  
	if(Pwm>7000)Pwm=7000;
	if(Pwm<-7000)Pwm=-7000;
	 return Pwm;                                          
}

/—————————————————————————————-

定时器周期：72乘10的6次方/（99+1）/（7199）=10ms

Timer1_Init(99,7199);           //=====定时中断初始化 

/————————————————————————————

定时器中断函数

处理

int TIM1_UP_IRQHandler(void)  
{    
	if(TIM_GetFlagStatus(TIM1,TIM_FLAG_Update)==SET)//5ms定时中断
	{ 
	  //清除中断标志位
	  TIM_ClearITPendingBit(TIM1,TIM_IT_Update);
      //从编码器计数值中获得位置（单位时间读取编码器计数）
      Encoder=Read_Velocity(4);
      //积分得到位置
      Position+=Encoder;    
      //位置PID控制器
      Position_Moto=Position_PID(Position,Target_Position);
      //限制位置环要输出的的速度<=速度环
	  limit_a=Xianfu(Position_Moto,myabs(Target_Velocity));
	  //速度PID控制器
	  Moto=Incremental_PI(Encoder,limit_a);		
	  //PWM限幅
	  Moto=Xianfu(Moto,7000);                          
	  Set_Pwm(Moto);
     } 
}