手把手教你搭建ROS阿克曼转向小车之（转向舵机驱动代码编写）

        上一篇文章介绍了小车的硬件构成，这篇文章则对转向舵机的驱动代码编写进行详细讲解。在控制舵机前，我们需要先了解舵机的工作原理。舵机的伺服系统由可变宽度的脉冲来进行控制，控制线是用来传送脉冲的。脉冲的参数有最小值，最大值，和频率。一般而言，舵机的基准信号都是周期为20ms，宽度为1.5ms。这个基准信号定义的位置为中间位置。舵机有最大转动角度，中间位置的定义就是从这个位置到最大角度与最小角度的量完全一样。最重要的一点是，不同舵机的最大转动角度可能不相同，但是其中间位置的脉冲宽度是一定的，那就是1.5ms。角度是由来自控制线的持续的脉冲所产生。这种控制方法叫做脉冲调制。脉冲的长短决定舵机转动多大角度。例如：1.5毫秒脉冲会到转动到中间位置（对于180°舵机来说，就是90°位置）。当控制系统发出指令，让舵机移动到某一位置，并让他保持这个角度，这时外力的影响不会让他角度产生变化，但是这个是由上限的，上限就是他的最大扭力。除非控制系统不停的发出脉冲稳定舵机的角度，舵机的角度不会一直不变。当舵机接收到一个小于1.5ms的脉冲，输出轴会以中间位置为标准，逆时针旋转一定角度。接收到的脉冲大于1.5ms情况相反。不同品牌，甚至同一品牌的不同舵机，都会有不同的最大值和最小值。一般而言，最小脉冲为1ms，最大脉冲为2ms。

        根据舵机的控制原理可以知道，我们需要让控制器产生20ms周期的占空比，然后再改变其占空比即可控制舵机转动指定的角度。控制器的PWM输出引脚如下图所示：

        所以在代码中我们需要先对端口和PWM进行初始化设置，相关的代码在 HwConfig/hw_STM32F40x.c中：

void ServoInit(uint8_t sName_t){
	GPIO_TypeDef* SERVO_PORT[SERVOn] = {STARBOT_SERVO1_GPIO_PORT, STARBOT_SERVO2_GPIO_PORT,STARBOT_SERVO3_GPIO_PORT,STARBOT_SERVO4_GPIO_PORT};
	TIM_TypeDef*  SERVO_TIM[SERVOn] = {STARBOT_SERVO1_TIM, STARBOT_SERVO2_TIM,STARBOT_SERVO3_TIM,STARBOT_SERVO4_TIM};
	const uint32_t  SERVO_PORT_CLK[SERVOn] = {STARBOT_SERVO1_GPIO_CLK, STARBOT_SERVO2_GPIO_CLK,STARBOT_SERVO3_GPIO_CLK,STARBOT_SERVO4_GPIO_CLK};
	const uint16_t  SERVO_PIN[SERVOn] = {STARBOT_SERVO1_PIN, STARBOT_SERVO2_PIN,STARBOT_SERVO3_PIN,STARBOT_SERVO4_PIN};
	const uint32_t  SERVO_TIM_CLK[SERVOn] = {STARBOT_SERVO1_TIM_CLK, STARBOT_SERVO2_TIM_CLK,STARBOT_SERVO3_TIM_CLK,STARBOT_SERVO4_TIM_CLK};
	const uint32_t  SERVO_AF_TIM[SERVOn] = {STARBOT_SERVO1_AFTIM, STARBOT_SERVO2_AFTIM,STARBOT_SERVO3_AFTIM,STARBOT_SERVO4_AFTIM};
	const uint32_t  SERVO_SOU_PIN[SERVOn] = {STARBOT_SERVO1_SOUPIN, STARBOT_SERVO2_SOUPIN,STARBOT_SERVO3_SOUPIN,STARBOT_SERVO4_SOUPIN};
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef TIM_BaseInitStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;
	RCC_AHB1PeriphClockCmd(SERVO_PORT_CLK[sName_t], ENABLE);
	if(sName_t == SERVO1 || sName_t == SERVO2)
		RCC_APB2PeriphClockCmd(SERVO_TIM_CLK[sName_t], ENABLE);
	else
		RCC_APB1PeriphClockCmd(SERVO_TIM_CLK[sName_t], ENABLE);
	GPIO_PinAFConfig(SERVO_PORT[sName_t],SERVO_SOU_PIN[sName_t],SERVO_AF_TIM[sName_t]); //GPIO复用为定时器X输出
	GPIO_InitStructure.GPIO_Pin     = SERVO_PIN[sName_t];
	GPIO_InitStructure.GPIO_Mode 	  = GPIO_Mode_AF;//复用功能
	GPIO_InitStructure.GPIO_Speed 	= GPIO_Speed_100MHz;	//速度100MHz
	GPIO_InitStructure.GPIO_OType 	= GPIO_OType_PP; //推挽复用输出
	GPIO_InitStructure.GPIO_PuPd 	= GPIO_PuPd_DOWN; //下拉
	GPIO_Init(SERVO_PORT[sName_t], &GPIO_InitStructure);

	TIM_BaseInitStructure.TIM_Period                = 19999;
	if(sName_t == SERVO1 || sName_t ==SERVO2)
	    TIM_BaseInitStructure.TIM_Prescaler         = 167;
	else
	    TIM_BaseInitStructure.TIM_Prescaler         = 83;
	TIM_BaseInitStructure.TIM_ClockDivision         = TIM_CKD_DIV1;
	TIM_BaseInitStructure.TIM_CounterMode           = TIM_CounterMode_Up;
	TIM_BaseInitStructure.TIM_RepetitionCounter     = 0;

	TIM_TimeBaseInit(SERVO_TIM[sName_t], &TIM_BaseInitStructure);

	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; 
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0; 
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; 

	if(sName_t == SERVO2 || sName_t ==SERVO3)
	{
		TIM_OC1Init(SERVO_TIM[sName_t], &TIM_OCInitStructure);
		TIM_OC1PreloadConfig(SERVO_TIM[sName_t], TIM_OCPreload_Enable);
	}

	if(sName_t == SERVO1 || sName_t ==SERVO4)
	{
		TIM_OC2Init(SERVO_TIM[sName_t], &TIM_OCInitStructure);
		TIM_OC2PreloadConfig(SERVO_TIM[sName_t], TIM_OCPreload_Enable);
	}

	TIM_ARRPreloadConfig(SERVO_TIM[sName_t], ENABLE);

//	TIM_CtrlPWMOutputs(SERVO_TIM[sName_t], ENABLE);
	TIM_Cmd(SERVO_TIM[sName_t], ENABLE);
}

        其中PE5和PE6对应的是定时器9的CH1和CH2，定时器9的时钟为168兆，PB14和PB15对应的是定时器12的CH1和CH2，定时器12的时钟为84兆，所以代码里Servo1和Servo2的分频值是167、Servo3和Servo4的分频值是83，这样给到定时器的频率就是1M即1us，计数值给定19999，1us*（19999+1）= 20ms，这样即可得到舵机所需要的控制脉冲频率。然后就是设定他的占空比值从而实现改变舵机的角度。具体代码在Middlewares/servo.cpp中：

uint16_t Servo::pos(uint16_t angle)
{ 
	float angle_s = 0.0;
	angle_s =(((angle+setData)*1000.0)/90.0) + 500;
	if(angle_s<500)
		angle_s = 500;
	if(angle_s>2500)
		angle_s = 2500;
	TIM_SetCompareX(SERVO_TIM[this->servo],SERVOx_CHANNEL[this->servo],angle_s);
	return ((int)angle_s);
}

         其中setData是可调整的初始转向值，如果我们安装的舵机角度不正可以调整这个参数，不需要改动硬件。