简易云台制作记录（内含MPU6050角度的求法）_mpu6050水平角度

开发板:STM32F103RCT6最小系统开发板
舵机 ：SG90
运动处理传感器 ： MPU-6050

云台

云台是安装、固定摄像机的支撑设备，固定云台可以用来调整摄像机的水平和俯仰的角度，电动云台由电动机接受来自控制器的信号精确的定位，可以扫描监视区域。

舵机

舵机是一种位置（角度）伺服的驱动器，适用于需要角度不断变化并可以保持的控制系统。

实现效果

在本项目中，MPU6050与舵机的结合无论处于什么位置，舵机上的扇叶始终保持与地面水平。

舵机处理：

舵机有三条导线：电源、地和控制线。
舵机的控制信号是PWM信号，利用占空比的变化，改变舵机的位置。
本次设计中使用TIM3_CH3输出PWM波形改变舵机的位置。代码的编写还是比较简单的，我首先使用按键控制改变PWM的占空比，观察舵机的而变化，脉冲方波信号的周期为20ms，按键每按下一次，高电平的时间增加0.1ms，测试发现当0.5ms时舵机位置开始变化，每按下一次，位置变化一丢丢，当到2.3的时候回旋转一圈，但没有返回刚开始的位置，并且此时舵机有一种卡住的感觉，当高电平的时间继续增加到2.5ms时，舵机恢复到刚开始的位置，具体的原因我还没有搞清楚。
这里还有一个需要注意的问题，可能也是我习惯不好，刚开始我搞了一会儿发现舵机的变化极其不规律，因为我平常只使用烧录器给开发板供电。而舵机需要的电压在4-6V,虽然插在了5V的引脚上，但供电仍然不够，之后使用USB线给开发板供电之后才测试出来了这些结果。

MPU-6050分析

RA是要读或写的寄存器的地址。
IIC协议：


本项目使用软件模拟I2C的电平变化传输数据。需要编写出起始信号、终止信号、应答、非应答、传或接收一个字节、传输或接受多个字节的函数，这些函数网上很多，也并不复杂，因此不在赘述。
当MPU6050做从机时，当AD0为低电平时，MPU6050的地址为0x68，为高电平时为0x69。也可以通过读WHO_AM_I寄存器得到器件ID，
下面列出一下常用的MPU6050寄存器的地址

/***********************************************************************/
//寄存器地址
#define MPU6050_PWR                 0x6b    //电源
#define MPU6050_SMPLRT_DIV          0x19    //取样时钟
#define MPU6050_CONFIG              0x1a    //低通滤波
#define MPU6050_GYRO_CONFIG		    0x1b    //陀螺仪量程(bit3,4)(0-250,1-500,2-1000,3-2000)
#define MPU6050_ACCEL_CONFIG        0x1c    //加速度计量程(bit3,4)(0-2g,1-4g,2-8g,3-16g)

#define MPU6050_RA_TITO_EN          0X23    //设备各种FIFO使能
#define MPU6050_RA_INT_ENABLE       0X38    //中断使能
#define MPU6050_RA_INT_PIN_CFG      0X37    //中断引脚配置寄存器
#define MPU6050_RA_USER_CTRL        0X6A    //使能FIFO缓存区、I2C主机模式喝主要接口
#define MPU6050_RA_PWR_MGMT_1       0X6B    //用户配置电源模式和时钟源
#define MPU6050_RA_PWR_MGMT_2       0X6C    //允许用户配置加速度计在低功耗模式下换器的频率


#define MPU6050_DEVICE_ID_REG       0x75    //WHO_AM_I寄存器

#define MPU6050_RA_ACCEL_XOUT_H     0x3B    //加速度
#define MPU6050_RA_ACCEL_XOUT_L     0x3C
#define MPU6050_RA_ACCEL_YOUT_H     0x3D
#define MPU6050_RA_ACCEL_YOUT_L     0x3E
#define MPU6050_RA_ACCEL_ZOUT_H     0x3F
#define MPU6050_RA_ACCEL_ZOUT_L     0x40
 
#define MPU6050_RA_TEMP_OUT_H       0x41    //温度
#define MPU6050_RA_TEMP_OUT_L       0x42
 
#define MPU6050_RA_GYRO_XOUT_H      0x43    //转速陀螺仪
#define MPU6050_RA_GYRO_XOUT_L      0x44
#define MPU6050_RA_GYRO_YOUT_H      0x45
#define MPU6050_RA_GYRO_YOUT_L      0x46
#define MPU6050_RA_GYRO_ZOUT_H      0x47
#define MPU6050_RA_GYRO_ZOUT_L      0x48

/****************************************************************************************************/
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

下面时MPU6050的初始化函数

//初始化MPU6050
//返回值: 0,成功
//        其他,错误代码
u8 MPU_Init(void)
{
    u8 res;
    IIC_Init();//初始化IIC总线
    I2C_Write_Byte(MPU6050_PWR,0X80);//复位MPU6050
    delay_ms(100);
    I2C_Write_Byte(MPU6050_PWR,0X00);//唤醒MPU6050
    MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
//    MPU_Set_Accel_Fsr(0); //加速度传感器 ±2g
    MPU_Set_Rate(50); //设置采样率50HZ
    I2C_Write_Byte(MPU6050_RA_INT_ENABLE,0X00); //关闭所有中断
    I2C_Write_Byte(MPU6050_RA_USER_CTRL,0X00);//I2C主模式关闭
    I2C_Write_Byte(MPU6050_RA_TITO_EN,0X00);//关闭FIFO
    I2C_Write_Byte(MPU6050_RA_INT_PIN_CFG,0X80);//INT引脚低电平有效
    res=I2C_Read_Byte(MPU6050_DEVICE_ID_REG);
    
    if(res==(u8)MPU_ADDR)//器件ID正确
    {
        I2C_Write_Byte(MPU6050_RA_PWR_MGMT_1,0X01);//设置CLKSEL,PLL X 轴为参考
        I2C_Write_Byte(MPU6050_RA_PWR_MGMT_2,0X00);//加速度陀螺仪都工作
        MPU_Set_Rate(50); //设置采样率为50HZ
    }else return 1;
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

初始化完成后读温度、陀螺仪、加速度计可得到相对应的值。
下面说一下云台的实现，通过读取到的这些数据可以得到三个角度：偏航角（Yaw）、俯仰角（Pitch）、横滚角（Roll）。
本次简易云台的实现只使用了俯仰角来实现。其实具体使用哪个角完全看你的MPU6050怎么和云台结合。通过我之前使用按键可以找到数字舵机旋转一周改变的PWM的范围，然后找到平衡时候的PWM，而云台转动的过程中，通过MPU6050得到的角度，乘上旋转一度需要的PWM弥补回来，就可以一直达到平衡的效果
代码实现非常简单

TIM_SetCompare3(TIM3,(2300-Angle_y*12));
1

这里2300和12都是通过自己的实验得到的。
下面简单介绍一下如何通过加速度计得到俯仰角和横滚角。

根据百度百科的定义，atan2(y,x)所表达的意思是坐标原点为起点，指向(x,y)的射线在坐标平面上与x轴正方向之间的角的角度。而俯仰角是机体坐标系y轴与水平面的夹角。因此利用图中最下面的公式便可以求出来俯仰角和横滚角，
而偏航角的求值我是利用陀螺仪求出角速度，然后利用定时器计算时间，对角速度积分求得的，最后，云台的实现要看MPU6050如何与舵机结合，然后才能确定需要使用哪一个角度，但这样求得的角度可能会没有那么准确，但对于一个简易的云台来说，完全足够了。经过亲自测试。效果还行。
以后有机会了，学习一下卡尔曼滤波之类的MPU6050的角度求法。

配套源码
https://download.csdn.net/download/weixin_44845857/20377503