四轮磁力爬行器

可以连接一个第三方数字传感器(如磁力计)，这样的话，就可以通过IIC接口输出一个9轴信号(链接第三方数字传感器才可以输出九轴信号，否则只有六轴信号)。当然，上面给大家展示的就是一个6轴的陀螺仪。更加方便的是，有了DMP，可以结合InvenSense公司提供的运动处理资料库，实现姿态解算。通过自带的DMP，可以通过IIC接口输出9轴融合演算的数据，大大降低了运动处理运算对操作系统的负荷，同时也降低了开发难度。其实，简单一句话说，陀螺仪就是测角速度的，加速度传感器就是测角加速度的，二者数据通过算法就可以得到PITCH、YAW、ROLL角了。

        MPU6050对陀螺仪和加速度计分别用了三个16位ADC，将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的。陀螺仪的可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。

        这里我给大家展示一下对应的.c和.h文件。这里的文件涉及到的比较多，博主在自学的时候学习了很多博主的代码，发现其中缺的东西很多，要不是少sysclock.h要不就是少sys.h，干脆这里我一并给大家放上一个链接，STM32F103C8T6驱动MPU6050并且在OLED屏幕上显示。

         这里我要给大家强调一下，如果delay函数不对的话，很可能会导致MPU6050的IIC时序不对而发生错误，所以大家最好是参照一下我的文件。此外要给大家强调的一点是如果大家用的管脚不是PB8 PB9而要修改其他的IO口的话，那么除了这里要修改。

         对应的IO操作方向也要注意，这里给大家提个醒，当初我就是没有仔细修改这里，而导致多次初始化失败，具体修改方法可以参考数据手册

        移植完成后，我们就可以通过Read_DMP(&Pitch,&Roll,&Yaw);这个函数来读取对应的角度，这个角度是由DMP结算过的，所以可以拿过来就直接用。

是否需要扶正，是否需要开机校准
        关于这个问题，我并没有出现过网上说的不扶正就不能开机，博主的小车只是用了MPU6050来准确的转一下90°角，所以也就没有过多关注这一块。是否需要开机校准，我觉得还是需要的，当你转过一个90°弯之后，我们可以通过重新让MPU6050初始化，来把转完弯后的方向设置为0°方向，这样方便下一次转弯，在代码逻辑上会减轻不少工作量。

        如果选择开机校准的话，那么我们就要让run_self_test()函数正常运行

static void run_self_test(void)
{
    int result;
    long gyro[3], accel[3];
 
    result = mpu_run_self_test(gyro, accel);
    if (result == 0x3) {
        /* Test passed. We can trust the gyro data here, so let's push it down
         * to the DMP.
         */
        float sens;
        unsigned short accel_sens;
        mpu_get_gyro_sens(&sens);
        gyro[0] = (long)(gyro[0] * sens);
        gyro[1] = (long)(gyro[1] * sens);
        gyro[2] = (long)(gyro[2] * sens);
        dmp_set_gyro_bias(gyro);
        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        dmp_set_accel_bias(accel);
        printf("setting bias succesfully ......\r\n");
    }
}

   让小车完成指令的过程中，采用了状态机的思路（FSM），当小车转弯一个90°之后，即将进入下一个状态的时候，我让MPU6050重新初始化，方便下一次转弯。

IIC_Init();
MPU6050_initialize();     //=====MPU6050初始化    
DMP_Init();
Yaw1 = 0;                //yaw1角度是缓解零飘问题的产物，这里也需要清零。
缓解yaw角零飘问题：
        这个问题其实困扰我很久，当时将MPU6050加到小车上的时候，我使用JLINK给板子烧录程序时，在OLED显示屏上看到的零飘很小，我猜测可能是有JLINK和电池一起供电的原因。我将JLINK断开后，使用电池给板子供电，零飘一秒能有2到3°，这就很离谱了，我从知乎一篇文章那里找到了一个软件的解决方案，那就是按照它的漂移规律，写一个可以随时间变换角度的函数来减缓这个趋势。我一并把代码粘出来，我将这个代码放到了TIM7定时器里，每10ms中断一次，把处理过的角度用OLED显示一下，大概是1分重才漂移1°，这就符合我们的要求了，而这个yaw1就是我们处理过后的角度。

if(++mpu_count == 6000)mpu_count = 0;
Read_DMP(&Pitch,&Roll,&Yaw);
yaw1 = Yaw - 0.001*mpu_count-0.49;
转弯函数：
        这个转弯函数是一个比较简单的写法，我通过两轮的差速来让小车进行转弯，同时利用MPU6050来时刻测算角度，满足指定角度之后，就停止差速转弯。

        这里要注意的一点是，MPU6050在旋转的过程中，如果到了180°左右，会突然变成0，这就说明这时候小车转弯转过了，就要给MPU6050加上一个角度大于0的限制，否则一旦跃变成0，那肯定会不停地旋转。

#include "stm32f10x.h"
#include "move.h"
#include "MPU6050.h"
#include "motor.h"
#include "FSM.h"
 
 
extern float Yaw1;
//float Target_Left = 85.000;
float bias_r;                    //左转函数用到的偏差
float bias_last_r;        //左转函数用到的上一次偏差
float bais_error;
float bias_l;
float bias_last_l;
float media_r;
unsigned char stop_flag;
void turn_right_90(float Yaw1,float Target_right)
{
    if(Yaw1 < Target_right)
    {    
            Motor3_forward(1499);
            Motor4_reverse(8499);
    }
    else
    {
        Move_stop();
    }
}
void turn_back(float Yaw1,float Target_right)
{
    if((Yaw1 < Target_right)&&(Yaw1 > 0))
    {    
            Motor3_forward(1499);
            Motor4_reverse(8499);
    }
    else if((Yaw1 > Target_right)||(Yaw1 < 0))
    {
        Move_stop();
    }

CAN设置

波特率：125K

帧类型：扩展帧

帧格式：数据帧

四轮磁力爬行器命令序列

0X00000901	0XAA	0X14	0X01	0X00	0X00	0X00	0X00	0XEE
扩展ID	帧头	功能类	具体动作	数据	数据	数据	数据	帧尾

前进：             09 01 AA 01 01 00 00 00 00 EE

后退：             09 01 AA 01 02 00 00 00 00 EE

停止：             09 01 AA 01 03 00 00 00 00 EE

左转：             09 01 AA 02 01 00 00 00 00 EE

右转：             09 01 AA 02 02 00 00 00 00 EE

停止：             09 01 AA 02 03 00 00 00 00 EE

计米清零 ：        09 01    AA 4A 03 00 00 00 00 EE

自动巡航开关：     09 01    AA 05 01 00 00 00 00 EE

自动巡航距离设定： 09 01    AA 05 02 XX XX XX XX EE

 设定的4个字节是单位是mm。4个字节左边为高字节，右边为低字节

上传温度：         01 09  AA 0A 10 XX XX XX XX EE

上传的4个字节是实际温度值的10倍。单位是℃，4个字节左边为高字节，右边为低字节

上传计米 ：        01 09  AA4A 01 XX XX XX XX EE

上传的4个字节单位是mm。4个字节左边为高字节，右边为低字节

上传巡航状态开：   01 09  AA 0B 10 00 00 00 01 EE

上传巡航状态关：   01 09  AA0B 10 00 00 00 02 EE

上传定长巡航距离： 01 09  AA 0D 01 XX XX XX XX EE//上传的设置的定长数值

上传的距离数据4个字节是单位是mm。4个字节左边为高字节，右边为低字节