[嵌入式]STM32F407VET6控制MG996R 180度舵机

STM32F407VET6控制MG996R 180度舵机

电赛之前想做一下板球控制系统，就先驱动一下舵机。

180与360度舵机

360度舵机与一般舵机的区别是：给一般舵机一个PWM信号，舵机会转到一个特定角度，而给360度舵机一个PWM信号，舵机会以一个特定的速度转动，类似与电机。但与电机不同的是，360舵机是闭环控制，速度控制稳定。
PWM 信号与360舵机转速的关系：

0.5ms----------------正向最大转速；

1.5ms----------------速度为0；

2.5ms----------------反向最大转速；

180度舵机为例：
0.5ms-------2.5%---------0度；

1ms ----------5%-------45度；

1.5ms--------7.5%--------90度；

2ms ----------10%-------135度；

2.5ms --------12.5-------180度；

180度舵机

如上，180度的舵机，角度值与占空比是一一对应的，不管你现在是多少角度，只要是一个给定的占空比，就会到达相应的角度。
以F407驱动，168MHz，TIM2~TIM7的时钟为84M，
舵机需要的周期为20ms，所以
psc = 84-1 = 83；
arr = 20000-1 = 19999；
这样就是 20000 * 84 / 84000000 = 0.02s = 20ms

使用的舵机为MG996R，
? 速度：4.8V@ 0.12±0.01sec/60°——6.0V@ 0.11±0.01sec/60°
? 扭力：4.8V@ 11kg-cm——6.0V@ 13kg-cm
? 电压：4.8V-6V
? 空载工作电流：220±20mA
? 堵转工作电流：2000±30mA
? 响应脉宽时间 ff5usec
? 角度偏差：回中差 ff1°，左右各 45° 误差 ff3°。
使用下面的共射极放大电路提供5V(4.8V到6V)的PWM控制舵机

由于共射极放大电路的反向功能，所以需要将占空比的值反过来，32输出的是5%占空比的方波，那么共射极输出的方波占空比就是95%

所以
0.5ms-------2.5%---------0度；40000-1000 = 39000；

1ms ----------5%-------45度；40000-2000 = 38000；

1.5ms--------7.5%--------90度；40000-3000 = 37000；

2ms ----------10%-------135度；40000-4000 = 36000；

2.5ms --------12.5%-------180度；40000-5000 = 35000；

CUBEMX配置生成代码（有部分TIM2定时器的配置）

/**
  ******************************************************************************
  * @file    tim.c
  * @brief   This file provides code for the configuration
  *          of the TIM instances.
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "tim.h"

/* USER CODE BEGIN 0 */

/******************************************************************************************************
TIM1、TIM8~TIM11的时钟为APB2时钟的两倍即168M，
TIM2~TIM7、TIM12~TIM14的时钟为APB1的时钟的两倍即84M。
Tout = ((arr+1)*(psc+1))/Tclk;
*******************************************************************************************************/


/* USER CODE END 0 */

TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim3;

/* TIM2 init function */
void MX_TIM2_Init(void)
{

  /* USER CODE BEGIN TIM2_Init 0 */

  /* USER CODE END TIM2_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM2_Init 1 */

  /* USER CODE END TIM2_Init 1 */
  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 16799;
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 4999;
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM2_Init 2 */
	HAL_TIM_Base_Start_IT(&htim2); //使能定时器 2 和定时器 2 更新中断
  /* USER CODE END TIM2_Init 2 */

}
/* TIM3 init function */
void MX_TIM3_Init(void)
{

  /* USER CODE BEGIN TIM3_Init 0 */

  /* USER CODE END TIM3_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};

  /* USER CODE BEGIN TIM3_Init 1 */

  /* USER CODE END TIM3_Init 1 */
  htim3.Instance = TIM3;
  htim3.Init.Prescaler = 41;
  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim3.Init.Period = 39999;
  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
  if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 1000;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM3_Init 2 */
	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1); //手动开启TIM3的PWM通道1
  /* USER CODE END TIM3_Init 2 */
  HAL_TIM_MspPostInit(&htim3);

}

void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
{

  if(tim_baseHandle->Instance==TIM2)
  {
  /* USER CODE BEGIN TIM2_MspInit 0 */

  /* USER CODE END TIM2_MspInit 0 */
    /* TIM2 clock enable */
    __HAL_RCC_TIM2_CLK_ENABLE();

    /* TIM2 interrupt Init */
    HAL_NVIC_SetPriority(TIM2_IRQn, 1, 2);
    HAL_NVIC_EnableIRQ(TIM2_IRQn);
  /* USER CODE BEGIN TIM2_MspInit 1 */

  /* USER CODE END TIM2_MspInit 1 */
  }
  else if(tim_baseHandle->Instance==TIM3)
  {
  /* USER CODE BEGIN TIM3_MspInit 0 */

  /* USER CODE END TIM3_MspInit 0 */
    /* TIM3 clock enable */
    __HAL_RCC_TIM3_CLK_ENABLE();
  /* USER CODE BEGIN TIM3_MspInit 1 */

  /* USER CODE END TIM3_MspInit 1 */
  }
}
void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
{

  GPIO_InitTypeDef GPIO_InitStruct = {0};
  if(timHandle->Instance==TIM3)
  {
  /* USER CODE BEGIN TIM3_MspPostInit 0 */

  /* USER CODE END TIM3_MspPostInit 0 */

    __HAL_RCC_GPIOA_CLK_ENABLE();
    /**TIM3 GPIO Configuration
    PA6     ------> TIM3_CH1
    */
    GPIO_InitStruct.Pin = GPIO_PIN_6;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  /* USER CODE BEGIN TIM3_MspPostInit 1 */

  /* USER CODE END TIM3_MspPostInit 1 */
  }

}

void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
{

  if(tim_baseHandle->Instance==TIM2)
  {
  /* USER CODE BEGIN TIM2_MspDeInit 0 */

  /* USER CODE END TIM2_MspDeInit 0 */
    /* Peripheral clock disable */
    __HAL_RCC_TIM2_CLK_DISABLE();

    /* TIM2 interrupt Deinit */
    HAL_NVIC_DisableIRQ(TIM2_IRQn);
  /* USER CODE BEGIN TIM2_MspDeInit 1 */

  /* USER CODE END TIM2_MspDeInit 1 */
  }
  else if(tim_baseHandle->Instance==TIM3)
  {
  /* USER CODE BEGIN TIM3_MspDeInit 0 */

  /* USER CODE END TIM3_MspDeInit 0 */
    /* Peripheral clock disable */
    __HAL_RCC_TIM3_CLK_DISABLE();
  /* USER CODE BEGIN TIM3_MspDeInit 1 */

  /* USER CODE END TIM3_MspDeInit 1 */
  }
}

/* USER CODE BEGIN 1 */
/*****************************************
舵机 20ms的周期，20000*8400/84000000 = 0.02
使用PWM1模式1，有效电平是高电平，所以
***********************************************/
//设置舵机占空比，
void TIM3_CH1_PWM_SetPulse(u16 cmp)
{
	__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1,cmp);
}
/************************************
0.5ms-------2.5%---------0度；40000-1000 = 39000；

1ms ----------5%-------45度；40000-2000 = 38000；

1.5ms--------7.5%--------90度；40000-3000 = 37000；

2ms ----------10%-------135度；40000-4000 = 36000；

2.5ms --------12.5%-------180度；40000-5000 = 35000；
*************************************************/
//设定舵机旋转角度
void Servo_ANGLE(float angle)
{
	float temp = 0;
	if(angle > 180)
		angle = 180;
	temp = (angle / 180) * Servo_Period + Servo_Static;
	TIM3_CH1_PWM_SetPulse(Servo_Duty - temp);
	
}

/*******************************舵机占空比设置函数*******************************************/
void set_steering_gear_dutyfactor(u16 dutyfactor)
{
	//对占空比所需要的CCR值做边界处理
	dutyfactor = 2.5/20*PWM_PERIOD_COUNT < dutyfactor ? 2.5/20*PWM_PERIOD_COUNT : dutyfactor;
	dutyfactor = 0.5/20*PWM_PERIOD_COUNT > dutyfactor ? dutyfactor : 0.5/20*PWM_PERIOD_COUNT;
	
	TIM3_CH1_PWM_SetPulse(PWM_PERIOD_COUNT - dutyfactor);
}

/*******************************设置舵机角度,传入角度值然后计算占空比,调用占空比设置函数*******************************************/
void set_steering_gear_angle(u16 angle_temp)
{
	//把角度转换成占空比所需要的计数值
	angle_temp = (0.5 + angle_temp / 180.0 * (2.5 - 0.5)) / 20.0 * PWM_PERIOD_COUNT;
	set_steering_gear_dutyfactor(angle_temp);
}

//定时器2中断回调函数
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if(htim->Instance == TIM2)
	{
		HAL_GPIO_TogglePin(LED0_GPIO_Port,LED0_Pin);
	}
}


/* USER CODE END 1 */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

封装舵机旋转角度函数

法1

（错误代码）

//设定舵机旋转角度
void Servo_ANGLE(u8 angle)
{
	u16 temp = 0;
	if(angle > 180)
		angle = 180;
	temp = (angle / 180) * Servo_Period + Servo_Static;
	
	TIM3_CH1_PWM_SetPulse(Servo_Duty - temp);
	
}

发现只有180度才是正常的，其他的都是2.5的占空比，也就是angle是0的时候对应的值，把各个值printf一遍才发现，是C语言基础知识不牢啊。angel只要设定的是<180度，那么一个小于180的数除以180一定是0啊。所以这样是错误的。

那么修改（改为float类型的变量）：

//设定舵机旋转角度
void Servo_ANGLE(float angle)
{
	float temp = 0;
	if(angle > 180)
		angle = 180;
	temp = (angle / 180) * Servo_Period + Servo_Static;
	TIM3_CH1_PWM_SetPulse(Servo_Duty - temp);
	
}

法2

/*******************************舵机占空比设置函数*******************************************/
void set_steering_gear_dutyfactor(u16 dutyfactor)
{
	//对占空比所需要的CCR值做边界处理
	dutyfactor = 2.5/20*PWM_PERIOD_COUNT < dutyfactor ? 2.5/20*PWM_PERIOD_COUNT : dutyfactor;
	dutyfactor = 0.5/20*PWM_PERIOD_COUNT > dutyfactor ? dutyfactor : 0.5/20*PWM_PERIOD_COUNT;
	
	TIM3_CH1_PWM_SetPulse(PWM_PERIOD_COUNT - dutyfactor);
}

/*******************************设置舵机角度,传入角度值然后计算占空比,调用占空比设置函数*******************************************/
void set_steering_gear_angle(u16 angle_temp)
{
	//把角度转换成占空比所需要的计数值
	angle_temp = (0.5 + angle_temp / 180.0 * (2.5 - 0.5)) / 20.0 * PWM_PERIOD_COUNT;
	set_steering_gear_dutyfactor(angle_temp);
}