我也是临危受命，在北京联系导师的时候，学校的老板就催我早点回去。其实，我心知肚明，那就是一个烂摊子，一个有关擦窗机器人的项目。老师们做的更多的是模仿，缺少思考，更缺少工程思维。

起初拿到这个项目，我就断言，这是一个几近于废掉的项目。混乱的代码，幼稚的算法，不经考虑的硬件，甚至没有调好的底层。

我只好给自己定一个需求目标:

做出机器人在窗户上的运动和路径规划。

第一天：

第一天先来重构程序结构，原来的程序是这样的：

暂且不提代码质量，混乱的结构，完全没有主次：

那么先把传感器的拿出来，再拿出控制层，哦，没有控制层，只好重建，这个工程完全没有闭环控制，那么，怎么可能走得直线吗！怎么可能按照一定的角度走，怎么可能路径规划！

再就是线程管理层，暂时先用比较简单地结构来重新写。结果发现，原来的人写了一堆的中断，根本没有系统意识，一个随便运行着的程序，正在等待着各种卡死。最后，通信层，也就是上位机交互调试，如果没有上位机配合，怎么调波形呢。

1.User - 调度管理,这一部分要详细说下。原来的作者几乎就是一个LOOP()解决所有问题，控制和各种资源调度全在这一个周期的循环里面，完全没有时序感，基本就是GG，所以我就做了这样的简单处理。

main:

Duty_Loop:

通过systick时钟来确定不同周期的任务

Duty_1ms:传感器处理部分，语音芯片，姿态器件，按键啦，气压计等等；

Duty_10ms:控制层，控制机器人的两个参数：basefly:油门也就是前进后退的基础速度；angel:机器人的角度；

Duty_50ms：任务调度器；

Duty_100ms：上位机通信；

这样，整个系统就建立起基础架构了！

MPU_lib - 姿态解算

（操蛋了，硬件设计第一大失误，就是采用了103c8t6，一款128k的芯片，根本顶不住计算量，而且这个系列，没有浮点数计算优化，所以只好通过配置mpu6050DMP解决问题，不过这也导致了后面的问题）

第一天，解决了姿态这一块的问题，原来的作者就是通过简单融合滤波，角度不稳定也不够准。陀螺仪求角度的问题主要是存在零点漂移，加速度计的问题就是有迟滞性。因此需要做融合滤波，不详细讲这里了。但是，这个片子的资源太小，做计算加上卡尔曼根本顶不住，只好用DMP模式，但是DMP模式也存在漏洞，进而导致后面不得不推倒重来。

DMP是啥？DMP就是MPU6050内置的计算单元，使用了官方的姿态计算算法，效果并不如人意。

以前做过DMP配置，配置完毕，读取四元数，通过四元数计算角度：

这里埋下了隐患。好，得到角度了。

直接上玻璃，不好调参数，那么先在地上调。其实效果是一样的。

第二天：

下面，就解决第二大问题，控制问题：

这就是控制层的函数，ElecPID.fdb是控制器的目标角度值。yaw_control就是PID控制函数了。

Control：

这一部分就很有难度了。我个人认为，我的PID函数是最简直漂亮的。采用了C语言 的指针函数这一技巧，非常的适合PID控制。还可以灵活切换不同的控制函数。PID控制使用了普通的线性pid控制器，控制机器人向某个方向按照一定的速度运转，如果角度可以做到0~360°的话，就属于全导航，但是鉴于我们姿态器件的算法能力，我们实现了0~180°方向的导航，其实这已经满足需求了。