STM32传感器模块编程实践(四)舵机+MPU6050陀螺仪模块融合云台模型

文章目录

一.概要

云台主要用来固定摄像头。准确地说，云台是一种可以多角度调节的支撑设备，类似于人的脖子可以支撑着脑袋，并通过转动来调节视野。
云台电机可以被用于相机、无人机和摄像机等设备上,以实现平滑稳定的拍摄效果，无人机在飞行过程中，尤其是遇到风力干扰或进行复杂飞行动作时，机身容易产生震动和晃动。而云台的存在可以有效地隔离这些抖动，保证挂载在上面的相机或摄像头能够拍摄出稳定、清晰的画面。这对于航拍、侦察、监测等需要高质量影像的任务至关重要。

本文就做了一个简单的云台模型，采用STM32F103C8T6单片机+舵机+陀螺仪模块，实现一个简易云台，随着陀螺仪模块倾斜多少度，舵机就转动到多少度。

二.实验模型原理

1.硬件连接原理框图

模型主要分为三部分:主芯片单元，倾斜测量单元，执行动作单元。

图中主控芯片为STM32F103C8T6单片机，倾斜测量为MPU6050陀螺仪模块，执行传感器为SG90舵机。

信号线连接:MPU6050陀螺仪模块信号脚SCL脚接到单片机的PB10脚，SDA脚接到单片机的PB11脚。SG90(180度)舵机的信号脚橙色线接到单片机的PA6脚。

2.控制原理

陀螺仪模块测量模块跟水平方向的夹角，如果大于0度，单片机驱动舵机也转到响应的角度，如果陀螺仪模块的倾斜角度没有变化，舵机角度保持不变，如果陀螺仪模块回到0度，舵机也相应转到0度。

陀螺仪模块测倾角原理：

MPU6050含3轴的加速度与3轴的陀螺仪，我们只要取单轴的加速度值，就能计算在某一方向上的倾斜角度。

舵机控制原理：
SG90舵机，首先，控制引脚是三根线，分别是GND(棕色)、VCC(红色)、PWM(黄色)，控制方式也是一样的PWM时序，具体的方法如下：

（1）采用PWM控制的方式来进行舵机的旋转
（2）舵机的控制需要MCU产生一个20ms周期的脉冲信号，以0.5ms到2.5ms的高电平来控制舵机的角度。
（3）高电平时间跟舵机旋转的角度对应关系:
0.5ms————-0度；对应函数中占空比为2.5%
1.0ms————45度；对应函数中占空比为5.0%
1.5ms————90度；对应函数中占空比为7.5%
2.0ms———–135度；对应函数中占空比为10.0%
2.5ms———–180度；对应函数中占空比为12.5%

三.实验模型控制流程

1.单片机先通过IIC口读取陀螺仪的角度。

2.计算陀螺仪的倾斜角度，如果有倾斜，驱动PA6引脚输出20ms周期控制舵机旋转到响应的角度。

3.如果倾斜角度回到0，控制舵机旋转到0度。

四.云台模型程序

板子与MPU6050陀螺仪模块用杜邦线连接：
板子G-----模块GND
板子3.3---模块VCC
板子B10---模块SCL
板子B11---模块SDA

板子与SG90舵机(180度舵机)用杜邦线连接：
板子5V----红色线
板子A6----橙色线
板子G-----棕色线

USB线需要接小系统板，给板子供5V。

打开STM32CubeMX软件,新建工程

Part Number处输入STM32F103C8，再双击就创建新的工程

配置下载口引脚

配置外部晶振引脚

配置系统主频

配置IIC引脚

配置PWM输出，定时器3通道1，周期20ms

配置工程文件名，保存路径，KEIL5工程输出方式

生成工程

用Keil5打开工程

添加代码

主要程序：

short Accel[3];
short Gyro [3];
short Temp;
float AccelData[3];//单位g
float GyroData[3];//单位mdps
/* USER CODE END PFP */


/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
uint16_t Pluse_Time=1000;//1毫秒脉冲，单位是us

typedef struct Angle
{
    double X_Angle;
    double Y_Angle;
    double Z_Angle;
    
} MPU6050_Angle;
MPU6050_Angle data;
double Angle_Old;
/****************************************************************************** 
* 函数介绍： 计算 x, y, z 三轴的倾角 
* 输入参数： 无 
* 输出参数： data：角度结构体 
* 返回值 ： 无 
******************************************************************************/
void MPU6050_Get_Angle(MPU6050_Angle *data)
{   
    /* 计算x, y, z 轴倾角，返回弧度值*/
    data->X_Angle = acos(AccelData[0]/1000);
    data->Y_Angle = acos(AccelData[1]/1000);
    data->Z_Angle = acos(AccelData[2]/1000);

    /* 弧度值转换为角度值 */
    data->X_Angle = data->X_Angle * 57.29577;
    data->Y_Angle = data->Y_Angle * 57.29577;
    data->Z_Angle = data->Z_Angle * 57.29577;
} 

/**
  * 函数功能: 读取MPU6050的加速度数据
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明: 无
  */ 
void MPU6050ReadAcc(short *accData)
{
    uint8_t buf[6];
		MPU6050_ReadData(MPU6050_ACC_OUT, &buf[0], 6);

	
    accData[0] = (buf[0] << 8) | buf[1];
    accData[1] = (buf[2] << 8) | buf[3];
    accData[2] = (buf[4] << 8) | buf[5];
}

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_I2C2_Init();//IIC初始化
  MX_TIM3_Init();//配置定时器，20ms周期PWM波,初始化高电平是1ms
	
	MPU6050_Init();//MPU6050初始化配置，中断使能配置
	if(MPU6050ReadID() == 0)//读取MPU6050 ID
	{	
	  while(1);
  }
  /* USER CODE BEGIN 2 */

   if (HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1) != HAL_OK)//PA6脚PWM输出
  {
    /* PWM generation Error */
    while(1);
  }
	
	Pluse_Time=500;//0.5ms高电平脉冲，用于控制舵机转到0度
	HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);//PWM停止输出
	MX_TIM3_Init();//重新初始化配置PWM波
	if (HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1) != HAL_OK)//PA6脚PWM输出
	{
	/* PWM generation Error */
	while(1);
	}
	HAL_Delay(300);//等待300ms
	HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);//PWM停止输出	

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
		MPU6050ReadAcc(Accel);//读取加速度数据
		for( int i=0;i<3;i++)
		{
		if(Accel[i]>=0)
		{
			AccelData[i]=Accel[i]*2000/32768;//转换成单位mg
		}else
		{
			AccelData[i]=-(-Accel[i]+1)*2000/32768;
		}	
		}	
	
		MPU6050_Get_Angle(&data);//计算倾角，通过X_Angle来控制舵机转角
	if(abs((int)Angle_Old-(int)data.X_Angle)>3)//当前角度与存储的角度差值大于3度，进行动作，主要是防止手抖动
	{	

			//以180度角度伺服为例，那么对应的控制关系是这样的（t为高电平时间）：
			//t=0.5ms（占空比2.5%）---------0°；
			//t=1.0ms（占空比5%）-----------45°；
			//t=1.5ms（占空比7.5%）---------90°；
			//t=2.0ms（占空比10%）---------135°；
			//t=2.5ms（占空比12.5%）-------180°；		
			Pluse_Time=500+((float)data.X_Angle/18)*200;//高电平时间=0.5ms+（角度/180°）×2ms
			HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);//PWM停止输出
			MX_TIM3_Init();//重新初始化配置PWM波
			if (HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1) != HAL_OK)//PA6脚PWM输出
			{
			/* PWM generation Error */
			while(1);
			}
			HAL_Delay(50);
			HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);//PWM停止输出	
  }
	Angle_Old=data.X_Angle;//把目前读到的角度值存下来，以便下次比较使用

  }
  /* USER CODE END 3 */
}

void MX_TIM3_Init(void)
{
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};

  htim3.Instance = TIM3;
  htim3.Init.Prescaler = 71;
  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim3.Init.Period = 19999;
  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  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 = Pluse_Time;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  HAL_TIM_MspPostInit(&htim3);

}

五.实验效果视频

演示效果

六.小结

融合了舵机，陀螺仪模块的控制，对STM32的单片机的定时器PWM功能，IIC通讯，陀螺仪的产品有更深的了解。

作者：光子物联单片机