本次还是延续以往的DIY风格，来给大家科普一下PID的知识》》》》》》》

首先申明一个关键点，PID调整是普遍应用在自动控制领域的一种调整方法，

举个栗子，如果机器在自动控制过程当中偏离了设定的轨道（或其他规定好的部分）

想要使它自动调整回原来的状态，就必须给相应动力机构一个调整量，关于这个调整量，

有人可能认为只要调整变化的量就好了啊，变多少调整多少呗，但很遗憾这样是绝对

不可能成功的。。。。。机器会疯狂的抽搐。。。。。。。。

于是，在一般的调整（又称比例调整，既是P调整）之上，添加了I（积分）和D（微分）调整

在产生一个变化量的同时，在变化量上取一个比例P调整，对所有的变化量求积分，得到变化总量，加入调整，

对该时刻变化量求微分，得到变化趋势，加入调整。由此得到输入到动力机构的总调整量，这样，机器才会

平稳的调整回去。

理论部分结束

》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》

本文以DIY方向为主，还是先来看看在实际DIY当中我们是如何做的吧。

一般PID调整在自稳定制作上应用较多，像自稳定小车，四轴无人机，自稳定云台等等

而他们的运作原理一般都是保持某一个平面的水平，在这种情况下，有一个绝佳的处理方案。

首先来看应用的模块——————MPU6050

它内部集成了两个关键组件，六轴传感器组，和姿态解算模块

这就使得它可以通过读取六轴传感器的数据并计算出一组至关重要的数据————模块所在平面的

俯仰角，旋转角，和侧滚角，（形象化命名），这时候我们就有了一个标准平面了，下面通过读取

平面的角度变化，就可以对相关组件进行实时调整了，这就是大多数自稳定机器人制作的基本原理。

为了保持该虚拟平面的水平，我们要不断的读取平面的细小角度变化，并且以PID的方式反馈到动力

输出组件上。苦逼的故事就从此开始了》》》》》》》》》》》》》》》》》

由于PID算法的不同，达到自稳定的要求可能也不同，我记得一个比较苦逼的哥们调整一个自稳定小车

调整了近两个月。。。。。。。。。小车才站起来。。。。。。。。。。

一般的PID算法需要调整P I D三个变量的权重，来达到稳定无抖动的目的，但这样的方法耗时耗力，

笔者最近一直在硬钢PID，之后会更新一期算法专题，谈谈PID算法的升级版。。。。。。。。。

》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》

附上32单片机的PID算法实例一二，以及MPU6050的32单片机驱动。

1.MPU6050驱动

//IIC写一个字节 
//reg:      寄存器地址
//data:     数据
//返回值:  0,正常
//          其他,错误代码
u8 IIC_Write_Byte(u8 reg,u8 data)
{
    IIC_Start();
    IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令 
    if(IIC_Wait_Ack())  //等待应答
    {
        IIC_Stop(); 
        return 1;       
    }
    IIC_Send_Byte(reg); //写寄存器地址
    IIC_Wait_Ack();     //等待应答 
        IIC_Send_Byte(data);//发送数据
    if(IIC_Wait_Ack())  //等待ACK
    {
        IIC_Stop();  
        return 1;        
    }        
    IIC_Stop();  
    return 0;
}
 
//IIC读一个字节 
//reg:寄存器地址 
//返回值:读到的数据
 
u8 IIC_Read_Byte(u8 reg)
{
    u8 res;
    IIC_Start();
    IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令 
    IIC_Wait_Ack();//等待应答
    IIC_Send_Byte(reg);//写寄存器地址
    IIC_Wait_Ack();//等待应答
    IIC_Start();
    IIC_Send_Byte((MPU_ADDR<<1)|1);//发送期间地址+读命令
    IIC_Wait_Ack();//等待应答
    res=IIC_Read_Byte(0);//读取数据，发送nACK
    IIC_Stop();//产生一个停止条件
    return res;
}
 
//IIC连续写
//addr:器件地址
//reg: 寄存器地址
//len: 写入长度
//buf: 数据区
//返回值: 0,正常
//              其他，错误代码
u8 IIC_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
    u8 i;
    IIC_Start();
    IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
    if(IIC_Wait_Ack())//等待应答
    {
        IIC_Stop();
        return 1;
    }
    IIC_Send_Byte(reg);//写寄存器地址
    IIC_Wait_Ack();//等待应答
    for(i=0;i<len;i++)
    {
        IIC_Send_Byte(buf[i]);//发送数据
        if(IIC_Wait_Ack())//等待ACK
        {
            IIC_Stop();
            return 1;
        }
    }
    IIC_Stop();
    return 0;
}
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取得长度
//buf:读取到的数据存储区
//返回值: 0,正常
//              其他，错误代码
u8 IIC_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
    IIC_Start();
    IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
    if(IIC_Wait_Ack())//等待应答
    {
        IIC_Stop();
        return 1;
    }
    IIC_Send_Byte(reg);//写寄存器地址
    IIC_Wait_Ack();//等待应答
    IIC_Start();
    IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令
    IIC_Wait_Ack();//等待应答
    while(len)
    {
        if(len==1) *buf=IIC_Read_Byte(0);//读数据，发送nACK
        else *buf=IIC_Read_Byte(1);//读数据，发送ACK
        len--;
        buf++;
    }
    IIC_Stop();//产生一个停止条件
    return 0;
}

3.2 MPU6050初始化
//初始化MPU6050
//返回值: 0,成功
//        其他,错误代码
u8 MPU_Init(void)
{
    u8 res;
    IIC_Init();//初始化IIC总线
    IIC_Write_Byte(MPU_PWR_MGMT1_REG,0X80);//复位MPU6050
    delay_ms(100);
    IIC_Write_Byte(MPU_PWR_MGMT1_REG,0X00);//唤醒MPU6050
    MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(0); //加速度传感器 ±2g
    MPU_Set_Rate(50); //设置采样率50HZ
    IIC_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
    IIC_Write_Byte(MPU_USER_CTRL_REG,0X00);//I2C主模式关闭
    IIC_Write_Byte(MPU_FIFO_EN_REG,0X00);//关闭FIFO
    IIC_Write_Byte(MPU_INTBP_CFG_REG,0X80);//INT引脚低电平有效
    res=IIC_Read_Byte(MPU_DEVICE_ID_REG);
    if(res==MPU_ADDR)//器件ID正确
    {
        IIC_Write_Byte(MPU_PWR_MGMT1_REG,0X01);//设置CLKSEL,PLL X 轴为参考
        IIC_Write_Byte(MPU_PWR_MGMT2_REG,0X00);//加速度陀螺仪都工作
        MPU_Set_Rate(50); //设置采样率为50HZ
    }else return 1;
    return 0;
}
 
//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
    return IIC_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}
 
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
    return IIC_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}
 
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_LPF(u16 lpf)
{
    u8 data=0;
    if(lpf>=188) data=1;
    else if(lpf>=98) data=2;
    else if(lpf>=42) data=2;
    else if(lpf>=42) data=3;
    else if(lpf>=20) data=4;
    else if(lpf>=10) data=5;
    else data=6; 
    return IIC_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器  
}
 
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败 
u8 MPU_Set_Rate(u16 rate)
{
    u8 data;
    if(rate>1000)rate=1000;
    if(rate<4)rate=4;
    data=1000/rate-1;
    data=IIC_Write_Byte(MPU_SAMPLE_RATE_REG,data);  //设置数字低通滤波器
    return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
}

3.3 读取MPU6050相关测得原始数据
//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    u8 buf[2]; 
    short raw;
        float temp;
        IIC_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); 
    raw=((u16)buf[0]<<8)|buf[1];  
    temp=36.53+((double)raw)/340;  
    return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    u8 buf[6],res;
    res=IIC_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
    if(res==0)
    {
        *gx=((u16)buf[0]<<8)|buf[1];  
        *gy=((u16)buf[2]<<8)|buf[3];  
        *gz=((u16)buf[4]<<8)|buf[5];
    }   
    return res;
}
 
//得到加速度值(原始值)
//ax,ay,az:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    u8 buf[6],res;  
    res=IIC_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
    if(res==0)
    {
        *ax=((u16)buf[0]<<8)|buf[1];  
        *ay=((u16)buf[2]<<8)|buf[3];  
        *az=((u16)buf[4]<<8)|buf[5];
    }   
    return res;;
}
原文：

https://blog.csdn.net/he__yuan/article/details/76559569 

以上是读取六轴传感器数据的部分。

》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》》

DMP（姿态解算核心）使用时需要打官方驱动，附上CSDN链接：

https://blog.csdn.net/zj490044512/article/details/83745684

我也在研究当中，望与诸君共勉