③【从0制作自己的ros导航小车：下位机篇】02、电机驱动、转速读取、PID控制

系列文章：
①【从0制作自己的ros导航小车：介绍及准备】
②【从0制作自己的ros导航小车：下位机篇】01、工程准备_标准库移植freertos
③【从0制作自己的ros导航小车：下位机篇】02、电机驱动、转速读取、PID控制
④【从0制作自己的ros导航小车：下位机篇】03、mpu6050偏航角获取
⑤【从0制作自己的ros导航小车：上、下位机通信篇】上、下位机串口DMA通信
⑥【从0制作自己的ros导航小车：上位机篇】01、里程计与坐标变换发布
⑦【从0制作自己的ros导航小车：上位机篇】02、ros1多机通讯与坐标变换可视化
⑧【从0制作自己的ros导航小车：上位机篇】03、添加urdf模型（发布各传感器与小车基坐标系之间的静态坐标变换）
⑨【从0制作自己的ros导航小车：上位机篇】04、使用gmapping建图
⑩【从0制作自己的ros导航小车：上位机篇】05、导航！

---

一、电机驱动

要想驱动电机光靠stm32的io口是不行的，需要用到电机驱动模块：TB6612也可以是别的比如L298N。这里使用TB6612，体积小性能强，刚好满足驱动两个电机的需求。
①接线
首先对电机进行接线，本文使用的直流减速电机接线如下：

驱动电机只需要使用到红、白两根，后续读取编码器时才需要用到中间四根线。本文使用TB6612电机驱动芯片接口如下：
驱动测试只需要接一边进行测试即可。PWMA接开发板的pwm输出口、AIN1和AIN2接开发板的正反转控制口、VM接5-12V电源、VCC接3.3或5V、STBY接3.3V、AO1和AO2接电机的红白口。接线完成后就是代码的编写。
②PWM相关初始化

PWM.c

#include "pwm.h"
//电机正反转控制的四个gpio初始化
void Motor_Gpio_Init(void)
{
  
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE);   //APB2使能
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); 
  GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);   
	
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;          //推挽输出
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3|GPIO_Pin_4;        
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	GPIO_ResetBits(GPIOB, GPIO_Pin_3);
	GPIO_ResetBits(GPIOB, GPIO_Pin_4);
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;          //推挽输出
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4|GPIO_Pin_5;        
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
}

void PWM_Init(void)
{
	/*开启时钟*/
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);			//开启TIM2的时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);			//开启GPIOA的时钟
	
	/*GPIO初始化*/
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);							
	
	/*配置时钟源*/
	TIM_InternalClockConfig(TIM3);		//选择TIM3为内部时钟，若不调用此函数，TIM默认也为内部时钟
	
	/*时基单元初始化*/
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;				//定义结构体变量
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;     //时钟分频，选择不分频，此参数用于配置滤波器时钟，不影响时基单元功能
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; //计数器模式，选择向上计数
	TIM_TimeBaseInitStructure.TIM_Period = 7199;                 //计数周期，即ARR的值
	TIM_TimeBaseInitStructure.TIM_Prescaler = 0;               //预分频器，即PSC的值
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;            //重复计数器，高级定时器才会用到
	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseInitStructure);             //将结构体变量交给        TIM_TimeBaseInit，配置TIM2的时基单元
	
	/*输出比较初始化*/ 
	TIM_OCInitTypeDef TIM_OCInitStructure;							//定义结构体变量
	TIM_OCStructInit(&TIM_OCInitStructure);                         //结构体初始化，若结构体没有完整赋值
	                                                                //则最好执行此函数，给结构体所有成员都赋一个默认值
	                                                                //避免结构体初值不确定的问题
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;               //输出比较模式，选择PWM模式1
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;       //输出极性，选择为高，若选择极性为低，则输出高低电平取反
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;   //输出使能
	TIM_OCInitStructure.TIM_Pulse = 0;								//初始的CCR值
	TIM_OC1Init(TIM3, &TIM_OCInitStructure);         
               //将结构体变量交给TIM_OC3Init，配置TIM3的输出比较通道1
							 
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;               //输出比较模式，选择PWM模式1
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;       //输出极性，选择为高，若选择极性为低，则输出高低电平取反
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;   //输出使能
	TIM_OCInitStructure.TIM_Pulse = 0;								//初始的CCR值
	TIM_OC2Init(TIM3, &TIM_OCInitStructure);  
	/*TIM使能*/
	TIM_Cmd(TIM3, ENABLE);			//使能TIM3，定时器开始运行
}

void motor_L_move(void)
{
    GPIO_SetBits(GPIOA, GPIO_Pin_4);
    GPIO_ResetBits(GPIOA, GPIO_Pin_5);
}


void motor_L_stop(void)
{
	GPIO_ResetBits(GPIOA, GPIO_Pin_4);
	GPIO_ResetBits(GPIOA, GPIO_Pin_5);
}

void motor_L_back(void)
{
    GPIO_SetBits(GPIOA, GPIO_Pin_5);
    GPIO_ResetBits(GPIOA, GPIO_Pin_4);
}

void motor_R_back(void)
{
	GPIO_ResetBits(GPIOB, GPIO_Pin_4);
	GPIO_SetBits(GPIOB, GPIO_Pin_3);
}

void motor_R_move(void)
{
	GPIO_SetBits(GPIOB, GPIO_Pin_4);
	GPIO_ResetBits(GPIOB, GPIO_Pin_3);
}

void motor_R_stop(void)
{
    GPIO_ResetBits(GPIOB, GPIO_Pin_3);
    GPIO_ResetBits(GPIOB, GPIO_Pin_4);
}

PWM.h

#ifndef __PWM_H
#define __PWM_H
#include "stm32f10x.h"  

void Motor_Gpio_Init(void);
void PWM_Init(void);

void motor_R_move(void);
void motor_R_back(void);

void motor_L_move(void);
void motor_L_back(void);

void motor_R_stop(void);//制动
void motor_L_stop(void);
#endif

不明白pwm原理的话，可以看江协科技的stm32教程，找到pwm这一节观看，代码复用性强。
上面代码的简单解释：这里初始化了四个引脚作为TB6612的逻辑控制，由两个引脚的0或者1能使输出颠倒从而实现电机正反转，具体正反转自己试试就行，你的小车哪个方向是向前，哪个方向是向后也由你定，根据确定后的值修改motor_R_move等四个函数控制电机的正反转。
③驱动电机转动
主函数中初始化PWM和gpio，然后选择前进或者后退或者其它，比如前进就在主函数中写motor_R_move();，这样就设置了右边电机正转，此时使用如下代码即可驱动电机转动：

TIM_SetCompare1(TIM3,500);//让tim3的CH1输出相应占空比pwm波

二、编码器

本文使用的电机自带霍尔式编码器，减速比为，一圈脉冲数为，这些是重要的参数，需要跟商家获取。霍尔式编码器的值读取同样参考江协科技的视频，有一章是编码器的使用。下面两端代码都属于encoder.c。
①编码器初始化

#include "encoder.h"

void Encoder_TIM2_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_ICInitTypeDef TIM_ICInitStruct;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//开启时钟
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//初始化GPIO--PA0、PA1，浮空输入
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_0 |GPIO_Pin_1;
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	
	TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
	TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseInitStruct.TIM_Period=65535;
	TIM_TimeBaseInitStruct.TIM_Prescaler=0;
	TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);
	
	TIM_ICStructInit(&TIM_ICInitStruct);//初始化输入捕获
	TIM_ICInitStruct.TIM_Channel = TIM_Channel_1;				//选择配置定时器通道1
	TIM_ICInitStruct.TIM_ICFilter = 0xF;							//输入滤波器参数，可以过滤信号抖动
	TIM_ICInit(TIM2, &TIM_ICInitStruct);							//将结构体变量交给TIM_ICInit，配置TIM3的输入捕获通道
	TIM_ICInitStruct.TIM_Channel = TIM_Channel_2;				//选择配置定时器通道2
	TIM_ICInitStruct.TIM_ICFilter = 0xF;							//输入滤波器参数，可以过滤信号抖动
	TIM_ICInit(TIM2, &TIM_ICInitStruct);							//将结构体变量交给TIM_ICInit，配置TIM3的输入捕获通道
	
  TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
	TIM_Cmd(TIM2, ENABLE);
}


void Encoder_TIM4_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_ICInitTypeDef TIM_ICInitStruct;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_6 |GPIO_Pin_7;
	GPIO_Init(GPIOB,&GPIO_InitStruct);
	
	TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);
	TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseInitStruct.TIM_Period=65535;
	TIM_TimeBaseInitStruct.TIM_Prescaler=0;
	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStruct);
	
	TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
	
	TIM_ICStructInit(&TIM_ICInitStruct);
	TIM_ICInitStruct.TIM_ICFilter=10;
	TIM_ICInit(TIM4,&TIM_ICInitStruct);
	
	TIM_ClearFlag(TIM4,TIM_FLAG_Update);
	TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);
	
	TIM_SetCounter(TIM4,0);

	TIM_Cmd(TIM4,ENABLE);
}

代码简单介绍：霍尔式电机编码器通过A、B相脉冲读取来实现后续速度计算，这里选择了TIM2与TIM4，AB相数据线的连接先于上面设定的io口连接起来即可，后面控制电机正转，然后观察读数，如果是负的，那就将AB两根线互换就行。
②读取编码器数据

/**********************
编码器
速度读取函数
入口参数：定时器
**********************/

int16_t Encoder1_get(void)
{
    int16_t Temp;
    Temp=TIM_GetCounter(TIM2);
    TIM_SetCounter(TIM2,0);
    return Temp;
}

int16_t Encoder2_get(void)
{
    int16_t Temp2;
    Temp2=TIM_GetCounter(TIM4);
    TIM_SetCounter(TIM4,0);
    return Temp2;
}

/**********************************************
函数功能：计算左轮速，正向速度为正值 ，反向速度为负值
返回  值：无
编码器参数：
                //一圈的脉冲数： 11 
                //减速比：21.3
                //轮子周长：20.7cm
             
               ① 转一圈车轮编码器实际读取到的脉冲数：
				int per_round =  11 * 4 * 21.3 = 937
				*4是因为编码器脉冲数据读取时设置的是4倍频，*21.3是因为车轮转一圈，相应的编码器就转减速比（20.7）圈，所以实际编码器读取的脉冲数如上式。
                ②最终以r/s为单位的公式 = 5ms内的脉冲个数 / per_round  / 5 *1000
                上式意思就是这一段时间内的脉冲个数除以一圈的脉冲个数再除以这一段时间，也就表示了1ms内转了多少圈，再乘以1000，就换算成了1s之内转了多少圈，也就是r/s。需要注意的是这个和m/s不是一个单位，将前面这个以r/s为单位的值乘上车轮的周长就换算乘了cm/s：
                ③速度(cm/s)=转速(r/s)×车轮周长(cm)或者速度(m/s)=转速(r/s)×车轮周长(m)
                
*********************************************/
int16_t Get_Right_Motor_Speed(int16_t leftSpeed)
{
			static int leftWheelEncoderNow    = 0;
			static int leftWheelEncoderLast   = 0;
			
			//记录本次左编码器数据
			leftWheelEncoderNow += Encoder2_get();
							
			//5ms测速            
			leftSpeed   =  20.7 * (leftWheelEncoderNow - leftWheelEncoderLast)*200 / 937;  //速度为cm/s
			//记录上次编码器数据
			leftWheelEncoderLast  = leftWheelEncoderNow;                    
      return leftSpeed;
      	
}

int16_t Get_Left_Motor_Speed(int16_t rightSpeed)
{
			static int rightWheelEncoderNow   = 0;
			static int rightWheelEncoderLast  = 0;        
			
			//记录本次右编码器数据
			rightWheelEncoderNow+= Encoder1_get();
							
         
			rightSpeed  =  20.7 * (rightWheelEncoderNow - rightWheelEncoderLast)*200/ 937;

			//记录上次编码器数据          
			rightWheelEncoderLast = rightWheelEncoderNow;    
      return rightSpeed;
}

void Timer_Init(void)
{
    NVIC_InitTypeDef NVIC_InitStruct;
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;

    // Enable the TIM1 clock and configure GPIO
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO, ENABLE);

    // Configure NVIC for TIM1 interrupt
    NVIC_InitStruct.NVIC_IRQChannel = TIM1_UP_IRQn;
    NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 4;
    NVIC_InitStruct.NVIC_IRQChannelSubPriority = 1;
    NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStruct);

    // TIM1 configuration
    TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);
    TIM_TimeBaseInitStruct.TIM_Prescaler = 7200 - 1; // Assuming 72MHz APB2 clock, 7200 prescaler gives 10kHz timer clock
    TIM_TimeBaseInitStruct.TIM_Period = 50 - 1;      // 10kHz / 50 = 200Hz, so 5ms period
    TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseInitStruct.TIM_RepetitionCounter = 0;
    TIM_TimeBaseInit(TIM1, &TIM_TimeBaseInitStruct);

    // Enable TIM1 update interrupt
    TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);

    // Start TIM1 counter
    TIM_Cmd(TIM1, ENABLE);
}

//这个回调函数放在主函数，需要定义全局变量left_speed_now和right_speed_now
void TIM1_UP_IRQHandler(void)
{
    if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET)
    {
			  TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
			  
			  left_speed_now = Get_Left_Motor_Speed(left_speed_now);
			  right_speed_now = Get_Right_Motor_Speed(right_speed_now);
    }
}

代码简单介绍：这里初始化了定时器1，周期是5ms，这样每过5ms进一次中断回调函数计算当前的速度。定义了左右轮速获取的函数，具体换算在代码中有给标注。

三、PID控制

这里直接参考这个博主的理解，pid的基础知识直接自己搜了看，从理解上来说是很容易的，pid就是要明确目标（setpoint），控制对象当前反馈量（current_value），以及pid输出。
对于我而言最困扰的就是如下这个问题：通过第二步已经可以读取到以cm/s为单位的速度信息了，现在需要控制电机转速至100cm/s，那么目标值就是100，此时当前反馈量就是编码器测得的速度，而输出量是什么呢？是pwm控制转速的数值！也就是前面那个TIM_SetCompare1(TIM3,500);中的500，这个容易理解，难的是为什么pid的输出能完美控制pwm的数值使得转速维持在100呢？实际上这个pid输出和速度是什么关系不必搞得很清楚，只需要知道设置了100为目标值之后，如果当前占空比低，也就是转速低，还未达到目标值，那么pid输出（pwm值）就大，当编码器反馈值越接近目标值时，pid输出就减小，以此类推从大幅度震荡到基本收敛。也就是只需要有三个量在，然后调pid参数，最终将目标值和当前值输入pid算法，得到最终输出。
①pid代码实现

typedef struct
{
  float Kp;                       //比例系数Proportional
  float Ki;                       //积分系数Integral
  float Kd;                       //微分系数Derivative
 
  float Ek;                       //当前误差
  float Ek1;                      //前一次误差 e(k-1)
  float Ek2;                      //再前一次误差 e(k-2)
  float LocSum;                   //累计积分位置
}PID_LocTypeDef;

float PID_Loc(float SetValue, float ActualValue, PID_LocTypeDef *PID)
{
  float PIDLoc;                                  //位置
	static int16_t err_lowout,err_lowout_last;
 
  PID->Ek = SetValue - ActualValue;
	err_lowout = 0.3* PID->Ek+0.7*err_lowout_last;	
	err_lowout_last=err_lowout;
	
  PID->LocSum += err_lowout;                         //累计误差
 
  PIDLoc = PID->Kp * PID->Ek + (PID->Ki * PID->LocSum) + PID->Kd * (PID->Ek1 - PID->Ek);
 
  PID->Ek1 = PID->Ek;  return PIDLoc;
}

②pid简单使用

//定义一个左轮pid结构体对象，并且设定pid的值，这里是我经过调试的值，一开始随便设，然后根据串口调试助手来自己调试
int16_t left_speed_now;
int16_t speed_left_SetPoint = 50;
PID_LocTypeDef left_speed_PID = {20,0.5,0.0,0.0,0.0,0.0};

void TIM1_UP_IRQHandler(void)
{
    TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
    
    left_speed_now = Get_Left_Motor_Speed(left_speed_now);
    TIM_SetCompare1(TIM3, PID_Loc(ABS(speed_left_SetPoint), ABS(left_speed_now), &left_speed_PID));
}

代码简单介绍：有了pid算法，就可以设置一个目标速度，然后在定时器回调函数中读取当前速度，与目标速度一起送入pid处理函数，返回值也就是pid输出，直接当作pwm控制电机转动。pid调参是个深不可测的话题，建议直接网上搜看别人如何调参，这里我还是参考上面那篇博客进行调参。