STM32控制两路直流电机_2

在上一篇博客中讲述了怎样使用STM32输出两路PWM波控制电机转动和使用定时器的编码器模块功能读取电机编码器的信息以获得电机的实时转速。
本篇博客内容包括：控制电机的转向,读取编码器数值,以及速度PID控制的编程方法。
（1）控制电机转向
由于我们使用的是TB6612芯片来驱动电机的运行，根据上一篇博客中所讲的控制方法可以编写一下代码实现对小车运动的控制：

/*
启动电机
*/
void motorStart(uint16_t pwma, uint16_t pwmb )
{
  GPIO_ResetBits(GPIOE,GPIO_Pin_7);
	GPIO_SetBits(GPIOE,GPIO_Pin_8);
	GPIO_ResetBits(GPIOE,GPIO_Pin_14);
	GPIO_SetBits(GPIOE,GPIO_Pin_13);
	GPIO_SetBits(GPIOE,GPIO_Pin_10);
	TIM1->CCR1 = pwma;
	TIM1->CCR2 = pwmb;
	TIM_Cmd(TIM1, ENABLE);
}

/*
停止电机运行
*/
void motorStop(void)
{
    GPIO_ResetBits(GPIOE,GPIO_Pin_10);
}

/*
控制小车前进
*/
void motorForward(void)
{
	  GPIO_ResetBits(GPIOE,GPIO_Pin_10);
	
	  GPIO_SetBits(GPIOE,GPIO_Pin_7);
	  GPIO_ResetBits(GPIOE,GPIO_Pin_8);
		GPIO_ResetBits(GPIOE,GPIO_Pin_13);
	  GPIO_SetBits(GPIOE,GPIO_Pin_14);
	
	  GPIO_SetBits(GPIOE,GPIO_Pin_10);
}


/*
控制小车后退
*/
void motorBackward(void)
{
	 GPIO_ResetBits(GPIOE,GPIO_Pin_10);
	
   	GPIO_ResetBits(GPIOE,GPIO_Pin_7);
	  GPIO_SetBits(GPIOE,GPIO_Pin_8);
	  GPIO_SetBits(GPIOE,GPIO_Pin_13);
	  GPIO_ResetBits(GPIOE,GPIO_Pin_14);
	
	  GPIO_SetBits(GPIOE,GPIO_Pin_10);
}


/*
控制小车左转
*/
void motorTurnLeft(void)
{
	GPIO_ResetBits(GPIOE,GPIO_Pin_10);
	
	GPIO_SetBits(GPIOE,GPIO_Pin_7);
	GPIO_ResetBits(GPIOE,GPIO_Pin_8);
	GPIO_SetBits(GPIOE,GPIO_Pin_13);
	GPIO_ResetBits(GPIOE,GPIO_Pin_14);
	
	GPIO_SetBits(GPIOE,GPIO_Pin_10);
}


/*
控制小车右转
*/
void motorTurnRight(void)
{
	GPIO_ResetBits(GPIOE,GPIO_Pin_10);
	
	GPIO_ResetBits(GPIOE,GPIO_Pin_7);
	GPIO_SetBits(GPIOE,GPIO_Pin_8);
	GPIO_ResetBits(GPIOE,GPIO_Pin_13);
	GPIO_SetBits(GPIOE,GPIO_Pin_14);
	
	GPIO_SetBits(GPIOE,GPIO_Pin_10);
}

(2)读取编码器的数值
已知车轮每转一圈编码器的计数值（本项目中轮子转一圈，编码器的计数值输出为1560），通过定时读取电机编码器的计数值，采用以下公式可以获取这段时间内电机的转速：
$$V_{motor}=\frac{编码器计数值}{定时器周期}$$

获取编码器计数值程序设计如下：

/*
获取编码器计数值，获取后的计数值存储在结构体motorInfo中
参数 x=3表示读取定时器3的计数值
     x=4表示读取定时器4的计数值
*/
extern MotorInfo     motorInfo;
void read_encoder(uint8_t x)
{
    switch(x)
		{
			case 3:
						 if(TIM3->CNT>=32678)
							 motorInfo.leftCnt=65535-TIM3->CNT;
						 else
							 motorInfo.leftCnt=TIM3->CNT;
					   TIM3->CNT=0;
						 break;
			case 4:
				     if(TIM4->CNT>=32678)
			          motorInfo.rightCnt=65535-TIM4->CNT;
		         else
		            motorInfo.rightCnt=TIM4->CNT;
						 TIM4->CNT=0;
						 break;
		} 
}

值得注意的是，每次读取完定时器的计数值之后都要把计数值清零，这样才能可靠地保证数据采集的正确性。

(3)小车速度的PID控制
根据上一篇博客中的控制框图，我们在此对小车两轮的速度分别加以控制，控制算法选择常用的数字式增量PID算法。

本PID控制器的输入是每个车轮的期望速度与实际速度的偏差，输出为电机PWM占空比的增量值。增量式PID控制算法可表示如下：
$$\Delta pwm=K_p\ast (err-er{r}_1)+K_i\ast err+K_d\ast (err-2\ast er{r}_1+er{r}_2)$$
$err$为本次误差，$er{r}_1$,$er{r}_2$前一次，前两次误差。
则控制电机的实际PWM为：
$$PW{M}_{current}=PW{M}_{previous}+\Delta pwm$$
PID控制算法如下：

//定义PID控制结构体
typedef struct Pid
{
  float kp;
	float ki;
	float kd;
	float A;
	float B;
	float C;
	float error_prev_1;
	float error_prev_2;
	float error_cur;
	float limit;
}PID;
//PID初始化
void pid_init(float kp,float ki,float kd,PID *pid)
{
   pid->kp=kp;
	 pid->ki=ki;
	 pid->kd=kd;
	
	 pid->A= pid->kp + pid->ki + pid->kd;
	 pid->B= -(2.0f*pid->kd + pid->kp);
	 pid->C= pid->kd;
	
	 pid->error_cur=0.0f;
	 pid->error_prev_1=0.0f;
	 pid->error_prev_2=0.0f;
	 pid->limit=0.4;
}
//执行PID计算
float pid(float error, PID * pid)
{
	 float delta_u;
	
	 pid->error_prev_2 = pid->error_prev_1;
	 pid->error_prev_1 = pid->error_cur;
	 pid->error_cur = error;
	
	 delta_u=pid->A * pid->error_cur + pid->B * pid->error_prev_1 + pid->C * pid->error_prev_2;
	
	 if(delta_u > pid->limit)
		 delta_u=pid->limit;
	else if(delta_u < -pid->limit)
		delta_u=-pid->limit;
	
	 return delta_u;
}

应用时，利用定时器周期读取编码器的计数值，然后迅速计算出速度偏差值，送入PID中进行计算，即可实现对电机速度的控制。但是PID控制器需要对参数Kp,Ki,Kd进行整定，这就需要进行大量的测试调试，不断更改这些参数的值，使其逼近最优的参数，才能达到理想的控制效果。
下一篇博客将介绍如何使用stm32驱动3路超声波传感器，测量距离值。