STM32F4速度环加角度环串级PID

小白学习笔记，欢迎指正~~~

整体思路

1.定时器配置编码器模式，实现对电机输出A,B相

void TIM2_ENCODER_Init(void)                      
{ 
  GPIO_InitTypeDef         GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_ICInitTypeDef        TIM_ICInitStructure;
 

   /*GPIO初始化*/

    (RCC_AHB1Periph_GPIOA,ENABLE); 	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);

	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1;//AB相接线
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_NOPULL;
	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;
    GPIO_Init(GPIOA,&GPIO_InitStructure);

    GPIO_PinAFConfig(GPIOA,GPIO_PinSource0,GPIO_AF_TIM2);
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource1,GPIO_AF_TIM2);

	TIM_TimeBaseStructure.TIM_Period=65535-1;
	TIM_TimeBaseStructure.TIM_Prescaler=1;
	TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Down;
	TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1; 
	TIM_TimeBaseInit(TIM2,&TIM_TimeBaseStructure);

	TIM_ICInitStructure.TIM_Channel=TIM_Channel_2|TIM_Channel_1;
	TIM_ICInitStructure.TIM_ICPolarity=TIM_ICPolarity_Rising;
	TIM_ICInitStructure.TIM_ICPrescaler=15;
	TIM_ICInitStructure.TIM_ICSelection=TIM_ICSelection_DirectTI;//IC1映射到TI上   
    TIM_ICSelection
	TIM_ICInitStructure.TIM_ICFilter=0;//输入通道的滤波参数
	TIM_ICStructInit(&TIM_ICInitStructure); 
	
	TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
	
  TIM_Cmd(TIM2,ENABLE);

	   
      
} 

2.定时器设置一段时间读取CNT的值

void TIM3_Int_Init()
{
  TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
  NVIC_InitTypeDef NVIC_InitStructure;
	
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);

  TIM_TimeBaseInitStructure.TIM_Period = 5000-1; 	//自动重装载值 //5ms记一次数
	TIM_TimeBaseInitStructure.TIM_Prescaler=84-1;  //定时器分频
	TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
	TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; 
	
	TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStructure);//初始化TIM3
	
	TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE); //允许定时器3更新中断
	NVIC_InitStructure.NVIC_IRQChannel=TIM3_IRQn; //定时器3中断
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x00; //抢占优先级1
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x00; //
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
	NVIC_Init(&NVIC_InitStructure);

	TIM_Cmd(TIM3,ENABLE); //使能定时器3
}

3.定时器输出PWM模式，输出经过PID算法后的电压值

//PWM输出
void TIM_PWM_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseInitStructure;
  TIM_OCInitTypeDef	TIM_OCInitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
	
	
	GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);
		GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
  GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	
	TIM_TimeBaseInitStructure.TIM_Period=3000-1;
	TIM_TimeBaseInitStructure.TIM_Prescaler=2-1;
	TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;
  TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure);

    
  TIM_OCInitStructure.TIM_OCMode=TIM_OCMode_PWM1;        // PWM模式2:CNT>CCR时输出有效
  TIM_OCInitStructure.TIM_OCPolarity=TIM_OCPolarity_High;// 设置极性-有效为高电平
  TIM_OCInitStructure.TIM_OutputState=TIM_OutputState_Enable;// 输出使能
  TIM_OC1Init(TIM4,&TIM_OCInitStructure);        
  TIM_OC1PreloadConfig(TIM4,TIM_OCPreload_Enable);        //使能预装载寄存器
    	

	TIM_Cmd(TIM4,ENABLE);




}

4.pid部分

void PID_Init(float A,float Kp,float Ki,float Kd,float target)
{	
error=target-A;   //目标速度小于电机速度，误差负值
integral+=error;    //误差累积
if(integral>2750)  {integral=2750;}   //积分限幅
if(integral<-2750) {integral=-2750;}	
p_out=Kp*error;
i_out=Ki*integral;
d_out=Kd*(error-error_last);
if(error>2000) {i_out=0;}      //积分分离
pwmout=p_out+i_out+d_out;
error_last =error;
if(pwmout>2750) {pwmout=2750;}
if(pwmout<-2750) {pwmout=-2750;}
change(pwmout);
return pwmout;
} 





void change(float a)
{
 if(a<0)
 {
	a=-a;
	TIM_SetCompare1(TIM4,a); 
	D0=0;
	D1=1;
 }
 else if(a>0)
 {
  TIM_SetCompare1(TIM4,a);
	D0=1;
	D1=0;	 
 }
 else
{
  TIM_SetCompare1(TIM4,0);
	D0=0;
	D1=0;
}
}

change()函数实现对pwmout正负数限制，电机反转时，pwmout为负值；电机正转，pwmout正值；

5.对速度解算

float encoder_now =0 ;
float encoder_last=0;
float e ;
float count;
float Read_counter(void)
{

	encoder_last = encoder_now;             
  encoder_now = TIM2->CNT;
	e = encoder_now - encoder_last;
		if(e > 32767) //
		{
			e =e-65535;//
		}
		else if(e<-32767)
		{
			e = e + 65535;//
		}		
		else 
		{
			e=e;//普通情况
		}	 
		
	 return e;
	}

float calculate_speed()
{
	  A = Read_counter(); 
	  n =A/(11*4*9.6);//电机一圈脉冲数*倍频*减速比
	  speed=n/0.005;       
	  return speed;
}

6.最后对PID实现

void TIM3_IRQHandler(void)  
{  
 if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET)
 {
	 float speed_temp;
	 static float angle_temp;
	 speed_temp=Read_counter(); 读取CNT
	 angle_temp+=speed_temp;     角度是对速度积分
	 real_angle=angle_temp*0.85227;   将CNT转为角度，用读取的CNT/一圈脉冲数*360
	 speed_temp= calculate_speed();
	 motorspeed=speed_temp*100;//扩大速度倍数
	 motorspeed=filter(motorspeed);//滤波后得到可用的速度
  
//角度环输出值为速度环输入值
	 pid_angle=pid_init(goal_angle,x,y,z,real_angle);
	 pid_speed=pid_init(pid_angle,13.5,1.25,36,motorspeed);
	

	 TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
 }
 }