我也是臨危受命，在北京聯系導師的時候，學校的老板就催我早點回去。其實，我心知肚明，那就是一個爛攤子，一個有關擦窗機器人的項目。老師們做的更多的是模仿，缺少思考，更缺少工程思維。
起初拿到這個項目，我就斷言，這是一個幾近於廢掉的項目。混亂的代碼，幼稚的算法，不經考慮的硬件，甚至沒有調好的底層。
我只好給自己定一個需求目標:
做出機器人在窗戶上的運動和路徑規划。
第一天：
第一天先來重構程序結構，原來的程序是這樣的：
暫且不提代碼質量，混亂的結構，完全沒有主次：
那么先把傳感器的拿出來，再拿出控制層，哦，沒有控制層，只好重建，這個工程完全沒有閉環控制，那么，怎么可能走得直線嗎！怎么可能按照一定的角度走，怎么可能路徑規划！
再就是線程管理層，暫時先用比較簡單地結構來重新寫。結果發現，原來的人寫了一堆的中斷，根本沒有系統意識，一個隨便運行着的程序，正在等待着各種卡死。最后，通信層，也就是上位機交互調試，如果沒有上位機配合，怎么調波形呢。
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°方向的導航，其實這已經滿足需求了。
PID控制見上圖，PID_Reset的作用是設置參數，PID_Calc則是控制輸出函數：
驅動電路如下：
基於PWM控制實現正轉和反轉的功率輸出，IN2\IN1接兩個PWM引腳。這里就是布線要求嚴格一點，PAD上要多鋪過孔，大電流通過的同時，減少分布參數，較小電磁干擾，然后電容C1比較關鍵，必須使用快恢復二極管，包括做開關電源也是這里處理不好，輸出就不好看。布線也是一個經驗問題。
之后，通過無線藍牙調試具體的參數，基本做到：轉向反應快，超調小，控制平滑，就可以。
做到這種曲線，波動和偏差盡可能小。但是，系統的調節能力比較弱，慢速系統的症結。
第三天：
我們弱化一下PID部分吧。直接將上層的任務調度層。因為實現時間太短。無法做復雜的任務調度管理。
我實現了上面兩個函數，就是一個單線程調度器，將任務函數f插入*Task中，然后TaskRun作為定期檢查函數，定期檢查函數運行：
實現三個功能：
1.是否觸發函數（基於時間還是基於事件）；
2.函數是否使能；
3.是否注銷函數；
然后運行函數隊列；
所以這里需要有一個非常嚴格的時鍾函數，為*Task做標定。