我也是临危受命，在北京联系导师的时候，学校的老板就催我早点回去。其实，我心知肚明，那就是一个烂摊子，一个有关擦窗机器人的项目。
老师们做的更多的是模仿，缺少思考，更缺少工程思维。
起初拿到这个项目，我就断言，这是一个几近于废掉的项目。混乱的代码，幼稚的算法，不经考虑的硬件，甚至没有调好的底层。
我只好给自己定一个需求目标:
做出机器人在窗户上的运动和路径规划。
越到后来，就越觉得气愤和困难。
第一天：
第一天先来重构程序结构，原来的程序是这样的：
暂且不提代码质量，混乱的结构，完全没有主次：
那么先把传感器的拿出来，再拿出控制层，哦，没有控制层，只好重建，这个工程完全没有闭环控制，那么，怎么可能走得直线吗！怎么可能按照一定的角度走，怎么可能路径规划！
再就是线程管理层，暂时先用比较简单地结构来重新写。结果发现，原来的人写了一堆的中断，根本没有系统意识，一个随便运行着的程序，正在等待着各种卡死。最后，通信层，也就是上位机交互调试，如果没有上位机配合，怎么调波形呢。
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控制。还可以灵活切换不同的控制函数。