STM32传感器模块编程实践(四)舵机+MPU6050陀螺仪模块融合云台模型
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通讯，陀螺仪的产品有更深的了解。