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

文章目录

- 一.概要
- 二.实验模型原理
- - 1.硬件连接原理框图
  - 2.控制原理
- 三.实验模型控制流程
- 四.云台模型程序
- 五.实验效果视频
- 六.小结

一.概要

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

本文就做了一个简单的云台模型，采用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毫秒脉冲，单位是ustypedef 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波,初始化高电平是1msMPU6050_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);//等待300msHAL_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°）×2msHAL_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);}