目錄

一、控制系統(tǒng)設(shè)計(jì)

1.1機(jī)械結(jié)構(gòu)設(shè)計(jì)

1.2傳感系統(tǒng)設(shè)計(jì)

1.3執(zhí)行器設(shè)計(jì)

1.4控制算法設(shè)計(jì)

二、控制系統(tǒng)的制作與調(diào)試

2.1機(jī)械結(jié)構(gòu)的制作與調(diào)試

2.2電路系統(tǒng)的制作與調(diào)試

2.3控制程序的編寫與調(diào)試

三、控制系統(tǒng)的測試與分析

3.1測試方法

3.2測試數(shù)據(jù)與現(xiàn)象

3.3結(jié)果分析

一、控制系統(tǒng)設(shè)計(jì)

一個(gè)系統(tǒng)產(chǎn)品的制作，要細(xì)分為多個(gè)模塊，分模塊進(jìn)行工作與進(jìn)行調(diào)試。對(duì)平衡小車進(jìn)分析得到如下的架構(gòu)圖。

分析平衡小車在蹺蹺板上平衡需要控制的物理量：

圖1-2 ?平衡條件分析

1.1機(jī)械結(jié)構(gòu)設(shè)計(jì)

在實(shí)現(xiàn)小車機(jī)械結(jié)構(gòu)的設(shè)計(jì)過程中，我們考慮了若干種方案。最終在四輪，三輪中選擇了以前輪控制轉(zhuǎn)向的三輪車方案。

四輪的車身穩(wěn)定，符合主流，但是受限于材料，無法輕易搭建懸掛機(jī)構(gòu)，因此必須考慮差速控制轉(zhuǎn)向。但兩個(gè)舵機(jī)的速度不能保證完全一致，且轉(zhuǎn)向時(shí)會(huì)產(chǎn)生較大摩擦阻力，還會(huì)增加程序的設(shè)計(jì)難度，因此我們沒有采用這種方案。

三輪可能存在側(cè)翻問題，但設(shè)計(jì)時(shí)將重心壓得很低，因此不存在這種情況。同時(shí)僅用兩個(gè)圓周舵機(jī)，拆裝方便，易于控制，結(jié)構(gòu)穩(wěn)定，轉(zhuǎn)向時(shí)后輪的摩擦阻力也可以忽略不計(jì)。因此我們最終采用了這種方案。

對(duì)平衡小車需要的機(jī)械結(jié)構(gòu)進(jìn)行分析得到下圖：

圖1-3 ?平衡小車機(jī)械結(jié)構(gòu)的設(shè)計(jì)

小車實(shí)現(xiàn)的運(yùn)動(dòng)：通過左右電機(jī)的調(diào)整以及前左右輪的轉(zhuǎn)速控制就可以實(shí)現(xiàn)方向調(diào)整。

傳感器和執(zhí)行器的安裝位置：傳感器的安裝位置要遠(yuǎn)離電機(jī)，因?yàn)殡姍C(jī)在運(yùn)動(dòng)過程中會(huì)產(chǎn)生電磁干擾，對(duì)傳感器正常測量數(shù)據(jù)會(huì)有一定的影響；同時(shí)執(zhí)行器需要安裝在小車外形結(jié)構(gòu)的預(yù)留位置，方便排線布局。

執(zhí)行器控制機(jī)械結(jié)構(gòu)運(yùn)動(dòng)示意圖：

圖1-4 ?執(zhí)行器控制電機(jī)轉(zhuǎn)動(dòng)示意圖

機(jī)械結(jié)構(gòu)的設(shè)計(jì)要求如圖1-5所示。

圖1-5 ?機(jī)械結(jié)構(gòu)設(shè)計(jì)要求

1.2傳感系統(tǒng)設(shè)計(jì)

本次實(shí)驗(yàn)中我們主要使用了GY953九軸傳感器，其工作原理，是通過陀螺儀與加速度,磁場傳感器經(jīng)過數(shù)據(jù)融合算法最后得到直接的角度數(shù)據(jù)。該傳感器具有體積小、高性價(jià)比、支持串口通信格式 和SPI 通信格式的特點(diǎn)。

傳感系統(tǒng)主要設(shè)計(jì)要求包括以下兩點(diǎn)。第一，蹺蹺板的擺動(dòng)角度很小，因此傳感系統(tǒng)所得數(shù)據(jù)需要非常精確，且噪聲不能太大；第二，蹺蹺板擺動(dòng)速度很快，姿態(tài)數(shù)據(jù)變化也很快，所以傳感系統(tǒng)數(shù)據(jù)傳輸?shù)乃俣炔荒芴Ｍㄟ^設(shè)置GY9531的相關(guān)參數(shù)，這些要求很容易實(shí)現(xiàn)。GY953相關(guān)技術(shù)參數(shù)如表1-1所示。

下面是九軸傳感器的包含的各個(gè)模塊以及測量的物理量數(shù)據(jù)類型：

圖1-6 ?傳感器功能

表 1-1 ?GY953技術(shù)參數(shù)

1.3執(zhí)行器設(shè)計(jì)

在整體的硬件性能上，我們提出以下要求：方便轉(zhuǎn)動(dòng)方向的調(diào)節(jié)；保證足夠的爬坡能力，并且不發(fā)生打滑；保證小車以當(dāng)前方向行駛時(shí)不發(fā)生偏移或偏移量較少。基于硬件要求，我們提出以下三種方案:

方案一：前輪轉(zhuǎn)向，后輪驅(qū)動(dòng)。執(zhí)行器選擇一個(gè)180°舵機(jī)和一個(gè)360°舵機(jī)。其中，180°舵機(jī)控制前輪轉(zhuǎn)向，360°舵機(jī)通過齒輪與軸控制后輪轉(zhuǎn)動(dòng)。

方案二：前輪轉(zhuǎn)向，后輪驅(qū)動(dòng)，執(zhí)行器選擇一個(gè)180°舵機(jī)和兩個(gè)360°舵機(jī)。其中，180°舵機(jī)控制前輪轉(zhuǎn)向，兩個(gè)360°舵機(jī)分別單獨(dú)控制對(duì)應(yīng)后輪的轉(zhuǎn)動(dòng)。

方案三：后輪驅(qū)動(dòng)，差速轉(zhuǎn)向。執(zhí)行器選擇兩個(gè)360°舵機(jī)，分別單獨(dú)與左右后輪相連。

經(jīng)比較，方案一的優(yōu)點(diǎn)在于調(diào)節(jié)轉(zhuǎn)動(dòng)方向的靈敏性和行駛時(shí)較小的方向偏移，但爬坡能力相對(duì)較弱。方案二彌補(bǔ)了方案一驅(qū)動(dòng)能力不足的缺點(diǎn)，但由于左右舵機(jī)在制造時(shí)的差異，難以保證兩個(gè)后輪轉(zhuǎn)速相同，從而存在一定的行駛方向偏移。方案三使用差速轉(zhuǎn)向，小車轉(zhuǎn)向性能受物理環(huán)境影響較大，但調(diào)整方向的復(fù)雜性略低于方案一和方案二。最后，我們決定使用方案三，通過優(yōu)化小車的機(jī)械機(jī)構(gòu)來減輕驅(qū)動(dòng)的負(fù)載。

在使用執(zhí)行器（舵機(jī)）時(shí)，我們需要對(duì)兩個(gè)360°舵機(jī)通過PWM的占空比單獨(dú)控制它的轉(zhuǎn)速和轉(zhuǎn)向。

圖1-7 ?所使用的舵機(jī)示意圖

1.4控制算法設(shè)計(jì)

我們的控制器選用單片機(jī)STM32F401，控制的物理量主要是PWM信號(hào)該控制器的主要功能是：發(fā)送指令給九軸傳感器，接收九軸傳感器發(fā)送過來的數(shù)據(jù)，對(duì)數(shù)據(jù)進(jìn)行處理，產(chǎn)生PWM信號(hào)。

小車尋找蹺蹺板平衡的控制系統(tǒng)框圖如圖1-8所示。傳感器用來測量當(dāng)前小車的姿態(tài)參數(shù)，由控制器進(jìn)行數(shù)據(jù)的處理，輸出數(shù)據(jù)控制舵機(jī)的轉(zhuǎn)動(dòng)方向和速度，以此控制小車的前進(jìn)、后退及其速度，最終由小車在蹺蹺板上的運(yùn)動(dòng)，控制蹺蹺板擺動(dòng)，此時(shí)小車也隨著蹺蹺板擺動(dòng)，再將角度數(shù)據(jù)反饋給傳感器。最終實(shí)現(xiàn)蹺蹺板的平衡。

圖1-8 控制系統(tǒng)框圖

基于控制系統(tǒng)框圖，我們對(duì)控制系統(tǒng)提出以下性能要求：響應(yīng)速度較快；超調(diào)量較?。徽鹗幋螖?shù)較少。由于蹺蹺板觸地對(duì)整個(gè)系統(tǒng)的影響較大，所以控制系統(tǒng)對(duì)響應(yīng)速度和超調(diào)量具有較高要求。

在采用1.3中的方案三作為執(zhí)行器方案后，控制系統(tǒng)將受到以下外界干擾：蹺蹺板轉(zhuǎn)動(dòng)時(shí)慣性的影響，蹺蹺板轉(zhuǎn)動(dòng)軸摩擦力的影響等，同時(shí)，傳感器在使用過程中會(huì)隨時(shí)間產(chǎn)生較大的漂移量。慣性和摩擦的影響可以通過提高響應(yīng)速度和減小超調(diào)量來削弱，傳感器的漂移可以通過濾波算法進(jìn)行校正，

控制算法分為兩部分，傳感器信號(hào)處理與舵機(jī)驅(qū)動(dòng)。由于傳感器在測量數(shù)據(jù)時(shí)產(chǎn)生漂移，我們使用濾波算法進(jìn)行削弱其影響，其中，常見的濾波算法有均值濾波、互補(bǔ)濾波、卡爾曼濾波等。

均值濾波的優(yōu)點(diǎn)在于計(jì)算簡單，通過計(jì)算多次數(shù)據(jù)的平均值，將平均值作為真實(shí)值。

互補(bǔ)濾波是通過基于傳感器對(duì)角度、角速度、加速度的測量靈敏性不同而設(shè)計(jì)的濾波算法。由于加速度計(jì)的低頻特性較好，加速度計(jì)的角度可以直接得出，所以沒有累計(jì)誤差，長時(shí)間內(nèi)性能較好，可以通過高通濾波來抑制低頻。而陀螺儀的角速度積分由于誤差的積累，長時(shí)間的性能較差而短時(shí)間內(nèi)性能較好，通過低通來抑制高頻。互補(bǔ)濾波則需要選擇低通與高通的切換頻率。互補(bǔ)濾波的公式為

?卡爾曼濾波是一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法。由于觀測數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計(jì)也可看作是濾波過程。卡爾曼濾波需要對(duì)系統(tǒng)進(jìn)行建模，同時(shí)需要干擾噪聲的方差等數(shù)據(jù)。

比較三種濾波方法，但無法達(dá)到理想濾波效果?？柭鼮V波較為精確，但對(duì)建模的精確度較高，受模型準(zhǔn)確性的影響較大。同時(shí)，噪聲方差等數(shù)值的要求難以滿足，設(shè)計(jì)難度較大?；パa(bǔ)濾波具有較好的濾波效果而均值濾波計(jì)算簡單。綜合考慮，我們決定先使用均值濾波作為傳感器信號(hào)處理的濾波算法，后期拓展時(shí)會(huì)采用互補(bǔ)濾波。

在驅(qū)動(dòng)部分中，我們使用PID算法作為調(diào)平衡的核心算法。在PID算法中，定值和實(shí)際輸出值構(gòu)成控制偏差，將偏差按比例、積分和微分通過線性組合構(gòu)成控制量，對(duì)被控對(duì)象進(jìn)行控制。控制算法的流程圖如圖1-8所示。

圖1-8 ?控制算法的流程圖

二、控制系統(tǒng)的制作與調(diào)試

2.1機(jī)械結(jié)構(gòu)的制作與調(diào)試

小車骨架的搭建：

圖2-1 ?小車骨架外觀圖

小車運(yùn)動(dòng)件的安裝：

圖2-2 ?小車運(yùn)動(dòng)件的位置示意圖

小車成品樣貌：

圖2-3 小車成品樣貌圖

2.2電路系統(tǒng)的制作與調(diào)試

電路系統(tǒng)的搭建時(shí)，首先遇到的問題是引腳不夠的問題。這是由于STM32開發(fā)板的5V和GND引腳只有2個(gè)，而設(shè)計(jì)的電路中傳感器和兩個(gè)圓周舵機(jī)都需要一個(gè)5V供電。為了解決這個(gè)問題，我們利用洞洞板將STM32開發(fā)板的5V引腳通過杜邦線引出至其中一組排針，GND引腳引出至另一組排針。傳感器、圓周舵機(jī)的對(duì)應(yīng)引腳分別接到兩組排針上，達(dá)到拓展引腳的目的。在實(shí)際測試時(shí)，也遇到了杜邦線因小車震動(dòng)而脫落、杜邦線排布混亂的問題。因此，我們獲得的經(jīng)驗(yàn)就是電路系統(tǒng)的排線一定要牢固可靠，防止后期出現(xiàn)問題，浪費(fèi)時(shí)間檢查。

另外，我們遇到了電機(jī)無法旋轉(zhuǎn)的問題，后來經(jīng)過老師的講解了解到是電機(jī)死區(qū)的問題

?圖2-4 ?電機(jī)死區(qū)示意圖

下面是在電路系統(tǒng)的測試過程中遇到的問題以及解決方案的總結(jié)。

表2-1 ?電路系統(tǒng)的問題以及解決方案

經(jīng)過不斷調(diào)試和修改，最終我們設(shè)計(jì)的電路系統(tǒng)外觀和架構(gòu)如圖2-4和圖2-5所示。

圖2-5 電路系統(tǒng)外觀圖

圖2-6 電路系統(tǒng)架構(gòu)圖

2.3控制程序的編寫與調(diào)試

控制程序的模塊架構(gòu)圖如下：

圖8.控制程序模塊結(jié)構(gòu)圖

下面是每個(gè)模塊的實(shí)現(xiàn)代碼：

StartTaskUART1GY953（包含給九軸傳感器發(fā)送指令，接收九軸傳感器數(shù)據(jù)，對(duì)傳感器數(shù)據(jù)進(jìn)行處理，PID運(yùn)算等）

StartTaskMotorCtrl（包含根據(jù)PID響應(yīng)對(duì)電機(jī)轉(zhuǎn)速進(jìn)行調(diào)整，平衡指示燈閃爍，平衡一定時(shí)間后退等）

編寫程序之后，對(duì)應(yīng)于響應(yīng)的目的，我們會(huì)打印出輸入和輸出到電腦串口調(diào)試助手上進(jìn)行分析和調(diào)試。出現(xiàn)的問題以及解決方案：

表2-2 ?編寫程序過程中遇到的問題以及解決方案

三、控制系統(tǒng)的測試與分析

3.1測試方法

評(píng)價(jià)小車性能有以下幾個(gè)要素：調(diào)平衡的次數(shù)（前后移動(dòng)一次為調(diào)平衡一次）；調(diào)平衡的時(shí)間；調(diào)平衡總過程蹺蹺板平均傾角。其中，行駛角度調(diào)整時(shí)間代表180°舵機(jī)的靈敏性；調(diào)平衡的次數(shù)反映系統(tǒng)的震蕩特性；調(diào)平衡的時(shí)間反映系統(tǒng)的綜合性能；平均傾角反映系統(tǒng)的超調(diào)量。為測試濾波效果，測試工作將在濾波和不濾波兩種條件下進(jìn)行。

3.2測試數(shù)據(jù)與現(xiàn)象

九軸傳感器接收的歐拉角中的pitch角如圖3-1所示。

使用串口調(diào)試助手查看數(shù)據(jù)變化如圖3-2所示。

圖 3-2 ?串口助手監(jiān)測

小車平衡過程：

圖 3-3 ?小車平衡狀態(tài)示意圖

3.3結(jié)果分析

1）根據(jù)傳感器狀態(tài)調(diào)整運(yùn)動(dòng)狀態(tài)：

由于我們小組在實(shí)際演示中只完成了小車在蹺蹺板上前進(jìn)使蹺蹺板平衡這個(gè)最基本的任務(wù)，因此，我們只用到了九軸傳感器的歐拉角數(shù)據(jù)，具體為pitch值。當(dāng)pitch值為負(fù)值時(shí)，判定小車為‘頭高尾低’狀態(tài)，即未達(dá)到平衡點(diǎn)，故小車會(huì)前進(jìn)；當(dāng)pitch值為正時(shí)，判定小車為‘頭低尾高’的狀態(tài)，即已經(jīng)超過平衡點(diǎn)，故小車會(huì)后退。而當(dāng)角度進(jìn)入所設(shè)定的平衡角度范圍內(nèi)時(shí)，判定為小車已找到平衡點(diǎn)，電機(jī)會(huì)停止轉(zhuǎn)動(dòng)，小車靜止。

表3-1濾波前后效果對(duì)比

2）小車會(huì)在平衡角度范圍內(nèi)抽搐：

由于我們采用pid算法控制，而pid算法最后實(shí)現(xiàn)的理想結(jié)果是小車的運(yùn)動(dòng)收斂，即小車停止在平衡點(diǎn)且蹺蹺板平衡。而由于蹺蹺板軸承有摩擦、小車并沒有走標(biāo)準(zhǔn)的直線等因素的影響，小車很難達(dá)到完美的理想狀態(tài)，以小車向后退到達(dá)平衡點(diǎn)為例，由于軸承的摩擦等因素，蹺蹺板并不會(huì)立即產(chǎn)生角度變化，而是當(dāng)小車走過平衡點(diǎn)后，給蹺蹺板一個(gè)更大的力，蹺蹺板才會(huì)變動(dòng)。而由于pid算法對(duì)蹺蹺板角度變化的預(yù)測，小車會(huì)立即向前移動(dòng)。同理，當(dāng)向前到達(dá)平衡點(diǎn)時(shí)，蹺蹺板不會(huì)立刻產(chǎn)生反應(yīng)，而是當(dāng)經(jīng)過平衡點(diǎn)后，蹺蹺板才會(huì)變動(dòng)。由于算法對(duì)蹺蹺板運(yùn)動(dòng)的預(yù)測，小車會(huì)立即向后運(yùn)動(dòng)，如此反復(fù)。最后表現(xiàn)為小車在平衡角度范圍內(nèi)抽搐。

3）平衡十秒后倒車：

當(dāng)小車檢測到進(jìn)入平衡角度后，time_delay函數(shù)開始計(jì)時(shí)。當(dāng)計(jì)時(shí)未達(dá)到10秒前平衡狀態(tài)被破壞，則time_delay清零，等到再次達(dá)到平衡狀態(tài)后time_delay開始計(jì)時(shí)。當(dāng)time_delay計(jì)時(shí)到達(dá)十秒后，直接控制小車后退，到小車退出蹺蹺板。文章來源地址http://www.zghlxwxcb.cn/news/detail-737759.html

附錄：main.c

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2022 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
typedef union {
    uint16_t   U16Data;
    int16_t    I16Data;
    uint8_t    C8Data[2];
}DataJointUnion;
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define UART1_RECV_BUFF_LEN         11
#define UART1_TRANS_BUFF_LEN        64
#define UART2_SEND_BUFF_LEN         64
#define time                        100
#define M_PI                        3.1415926
#define M_COS(x)                    cos((x*M_PI)/180)
#define M_SIN(x)                    sin((x*M_PI)/180)
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
 TIM_HandleTypeDef htim2;

UART_HandleTypeDef huart1;
UART_HandleTypeDef huart2;

osThreadId defaultTaskHandle;
osThreadId myTask02Handle;
osThreadId myTask03Handle;
osThreadId myTask04Handle;
osSemaphoreId myBinarySem_UART1GY953RecvHandle;
osSemaphoreId myBinarySem_UART1ToUART2TransReadyHandle;
osSemaphoreId myBinarySem_UART2SendHandle;
/* USER CODE BEGIN PV */
uint8_t     UART1RecvBuff[UART1_RECV_BUFF_LEN];
uint8_t     UART1TransToUART2Buff[UART1_TRANS_BUFF_LEN];
uint16_t    UART1TransToUART2Size=0;
uint16_t    UART2SendSize=0;
uint8_t     UART2SendBuff[UART2_SEND_BUFF_LEN];
//int16_t     spdleft=500;
//int16_t     spdright=500;
int16_t deta_data=0;
int16_t pitch_deta=0;
int16_t spdleft=50;
int16_t spdright=50;
int16_t pitch_atti=0;
int16_t time_delay=0;
int16_t speed0=50;

int16_t pitch_times = 0;  //p角同號(hào)次數(shù)
int16_t pitch_times_ctl = 1000;  //平衡控制

int16_t xiangying_pid=0;//PID控制器控制量
const int FilterOrder = 21;
const double FilterHn[21] = {
        -0.01299253964226,  -0.0150332929185, -0.01207107368107,-0.002207438432842,
            0.01529274896489,  0.03957521406373,  0.06812913211038,  0.09713578873324,
             0.1221778815001,     0.13915124264,   0.1451573563434,     0.13915124264,
             0.1221778815001,  0.09713578873324,  0.06812913211038,  0.03957521406373,
            0.01529274896489,-0.002207438432842, -0.01207107368107,  -0.0150332929185,
           -0.01299253964226
};
double  MagnXRawDataFIFO[21];
double  MagnYRawDataFIFO[21];
double  MagnZRawDataFIFO[21];
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_TIM2_Init(void);
void StartDefaultTask(void const * argument);
void StartTaskUART1GY953(void const * argument);
void StartTaskUART2(void const * argument);
void StartTaskMotorCtrl(void const * argument);

/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  MX_TIM2_Init();
  /* USER CODE BEGIN 2 */

  /* USER CODE END 2 */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* Create the semaphores(s) */
  /* definition and creation of myBinarySem_UART1GY953Recv */
  osSemaphoreDef(myBinarySem_UART1GY953Recv);
  myBinarySem_UART1GY953RecvHandle = osSemaphoreCreate(osSemaphore(myBinarySem_UART1GY953Recv), 1);

  /* definition and creation of myBinarySem_UART1ToUART2TransReady */
  osSemaphoreDef(myBinarySem_UART1ToUART2TransReady);
  myBinarySem_UART1ToUART2TransReadyHandle = osSemaphoreCreate(osSemaphore(myBinarySem_UART1ToUART2TransReady), 1);

  /* definition and creation of myBinarySem_UART2Send */
  osSemaphoreDef(myBinarySem_UART2Send);
  myBinarySem_UART2SendHandle = osSemaphoreCreate(osSemaphore(myBinarySem_UART2Send), 1);

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* definition and creation of defaultTask */
  osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 128);
  defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);

  /* definition and creation of myTask02 */
  osThreadDef(myTask02, StartTaskUART1GY953, osPriorityIdle, 0, 128);
  myTask02Handle = osThreadCreate(osThread(myTask02), NULL);

  /* definition and creation of myTask03 */
  osThreadDef(myTask03, StartTaskUART2, osPriorityIdle, 0, 128);
  myTask03Handle = osThreadCreate(osThread(myTask03), NULL);

  /* definition and creation of myTask04 */
  osThreadDef(myTask04, StartTaskMotorCtrl, osPriorityIdle, 0, 128);
  myTask04Handle = osThreadCreate(osThread(myTask04), NULL);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* Start scheduler */
  osKernelStart();

  /* We should never get here as control is now taken by the scheduler */
  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 25;
  RCC_OscInitStruct.PLL.PLLN = 160;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief TIM2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM2_Init(void)
{

  /* USER CODE BEGIN TIM2_Init 0 */

  /* USER CODE END TIM2_Init 0 */

  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};

  /* USER CODE BEGIN TIM2_Init 1 */

  /* USER CODE END TIM2_Init 1 */
  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 80-1;
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 20000-1;
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 500;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM2_Init 2 */

  /* USER CODE END TIM2_Init 2 */
  HAL_TIM_MspPostInit(&htim2);

}

/**
  * @brief USART1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART1_UART_Init(void)
{

  /* USER CODE BEGIN USART1_Init 0 */

  /* USER CODE END USART1_Init 0 */

  /* USER CODE BEGIN USART1_Init 1 */

  /* USER CODE END USART1_Init 1 */
  huart1.Instance = USART1;
  huart1.Init.BaudRate = 115200;
  huart1.Init.WordLength = UART_WORDLENGTH_8B;
  huart1.Init.StopBits = UART_STOPBITS_1;
  huart1.Init.Parity = UART_PARITY_NONE;
  huart1.Init.Mode = UART_MODE_TX_RX;
  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART1_Init 2 */

  /* USER CODE END USART1_Init 2 */

}

/**
  * @brief USART2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART2_UART_Init(void)
{

  /* USER CODE BEGIN USART2_Init 0 */

  /* USER CODE END USART2_Init 0 */

  /* USER CODE BEGIN USART2_Init 1 */

  /* USER CODE END USART2_Init 1 */
  huart2.Instance = USART2;
  huart2.Init.BaudRate = 115200;
  huart2.Init.WordLength = UART_WORDLENGTH_8B;
  huart2.Init.StopBits = UART_STOPBITS_1;
  huart2.Init.Parity = UART_PARITY_NONE;
  huart2.Init.Mode = UART_MODE_TX_RX;
  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART2_Init 2 */

  /* USER CODE END USART2_Init 2 */

}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOC_CLK_ENABLE();
  __HAL_RCC_GPIOH_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET);

  /*Configure GPIO pin : PC13 */
  GPIO_InitStruct.Pin = GPIO_PIN_13;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

}

/* USER CODE BEGIN 4 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if(huart == &huart1)
    {
        osSemaphoreRelease(myBinarySem_UART1GY953RecvHandle);
        HAL_UART_Receive_IT(&huart1, UART1RecvBuff, UART1_RECV_BUFF_LEN);
    }
}

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
    if(huart == &huart2)
    {
        osSemaphoreRelease(myBinarySem_UART2SendHandle);
    }
}
void MotorCtrl(uint8_t chn, int16_t spd)
{
    if(spd*speed0<0){
        pitch_times_ctl = pitch_times;
        pitch_times = 0;
    }
    if(spd>1000){spd = 1000;}

    if(spd<-1000){spd = -1000;}
    switch(chn)
    {
        case 0 :
            spd = spd + 1500;
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, spd);  //范圍500~2500，對(duì)應(yīng)0.5ms~2.5ms
            break;
        case 1 :
            spd = -1 * spd + 1500;
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, spd);  //范圍500~2500，對(duì)應(yīng) ????????????0.5ms~2.5ms
            break;
        default:break;
    }
    speed0 = spd;

}
double  FilterCalculation(double newRawData, double *prawDataFIFO, double *pFirHn, int firOder) {
    int     i;
    double  result;

    i   = firOder-1;
    while(i) {
        prawDataFIFO[i]     = prawDataFIFO[i-1];
        i--;
    }
    prawDataFIFO[0]  = newRawData;

    result = 0;
    for(i=0; i<firOder; i++) {
        result = result + pFirHn[i]*prawDataFIFO[i];
    }

    return result;
}
/* USER CODE END 4 */

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void const * argument)
{
  /* USER CODE BEGIN 5 */
  /* Infinite loop */
  for(;;)
  {
    osDelay(1);
  }
  /* USER CODE END 5 */
}

/* USER CODE BEGIN Header_StartTaskUART1GY953 */
/**
* @brief Function implementing the myTask02 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTaskUART1GY953 */
void StartTaskUART1GY953(void const * argument)
{
  /* USER CODE BEGIN StartTaskUART1GY953 */
    uint8_t cmd0[]={0xA5,0x15,0xBA};// acceleration data
    uint8_t cmd1[]={0xA5,0x35,0xDA};// magnet data
    uint8_t cmd2[]={0xA5,0x45,0xEA};// Euler data(50Hz)
    uint16_t accx0 = 0;
    int16_t pitch0=0; //歐拉角初始pitch
    DataJointUnion  tempUnion;
    double firResult;
    int16_t pitch_deta_1=0;
    uint8_t isDirectTransToUART2=0;   //1將傳感器標(biāo)準(zhǔn)數(shù)據(jù)幀直接傳到電腦，0表示會(huì)將數(shù)據(jù)進(jìn)行處理

    osDelay(1000);

    //HAL_UART_Transmit(&huart1, cmd0, sizeof(cmd0), 100);  //獲取角速度數(shù)據(jù)
    //HAL_UART_Transmit(&huart1, cmd1, sizeof(cmd1), 100);  //獲取地磁角數(shù)據(jù)
    HAL_UART_Transmit(&huart1, cmd2, sizeof(cmd2), 100);   //獲取歐拉角數(shù)據(jù)
    //-----------------------------------------------------------------
    //pitch角度的PID控制
    int16_t Kp_pitch=10;
    int16_t Ki_pitch=0;
    int16_t Kd_pitch=120;
   //--------------------------------------------------------------------
    HAL_UART_Receive_IT(&huart1, UART1RecvBuff, UART1_RECV_BUFF_LEN);  //uart1接收傳感器數(shù)據(jù)
  /* Infinite loop */
    for(;;)
    {
        osSemaphoreWait(myBinarySem_UART1GY953RecvHandle, osWaitForever);
        UART1TransToUART2Size   = 0;
        if(isDirectTransToUART2) {
            memcpy(UART1TransToUART2Buff, UART1RecvBuff, UART1_RECV_BUFF_LEN);
            UART1TransToUART2Size   = UART1_RECV_BUFF_LEN;
        } else {
            switch(UART1RecvBuff[2]) {
                case    0x15    :  // 處理角速度數(shù)據(jù)
                    tempUnion.C8Data[1]     = UART1RecvBuff[4];
                    tempUnion.C8Data[0]     = UART1RecvBuff[5];
                    sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Ax:%05d ", tempUnion.U16Data);
//                    if(abs(accx0-tempUnion.U16Data)>150){
                    deta_data=accx0-tempUnion.U16Data;
                    if(deta_data>0)
                    {
                        deta_data=deta_data;
                    }
                    else{
                        deta_data=-deta_data;
                    }
                    accx0 = tempUnion.U16Data;

                    UART1TransToUART2Size   = UART1TransToUART2Size + 9;
                    tempUnion.C8Data[1]     = UART1RecvBuff[6];
                    tempUnion.C8Data[0]     = UART1RecvBuff[7];
                    //sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Ay:%05d ", deta_data);
                    sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Ay:%05d ", tempUnion.U16Data);
                    UART1TransToUART2Size   = UART1TransToUART2Size + 9;
                    tempUnion.C8Data[1]     = UART1RecvBuff[8];
                    tempUnion.C8Data[0]     = UART1RecvBuff[9];
                    sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Speed:%05d \r\n",tempUnion.U16Data);
                    UART1TransToUART2Size   = UART1TransToUART2Size + 9 + 2;
                    break;
                case    0x35    :  //處理獲取地磁角數(shù)據(jù)
                    tempUnion.C8Data[1]     = UART1RecvBuff[4];
                    tempUnion.C8Data[0]     = UART1RecvBuff[5];
                    sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Mx:%06d ", tempUnion.I16Data);
                    UART1TransToUART2Size   = UART1TransToUART2Size + 10;
                    tempUnion.C8Data[1]     = UART1RecvBuff[6];
                    tempUnion.C8Data[0]     = UART1RecvBuff[7];
                    sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "My:%06d ", tempUnion.I16Data);
                    UART1TransToUART2Size   = UART1TransToUART2Size + 10;
                    tempUnion.C8Data[1]     = UART1RecvBuff[8];
                    tempUnion.C8Data[0]     = UART1RecvBuff[9];
                    sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Mz:%06d \r\n", tempUnion.I16Data);
                    UART1TransToUART2Size   = UART1TransToUART2Size + 10 + 2;
                    break;

                case    0x45    :   //處理歐拉角數(shù)據(jù)
                    pitch_times++;
                    tempUnion.C8Data[1]     = UART1RecvBuff[4];
                    tempUnion.C8Data[0]     = UART1RecvBuff[5];
                    sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "x:%06d ", time_delay);
                    UART1TransToUART2Size   = UART1TransToUART2Size + 9;
                    tempUnion.C8Data[1]     = UART1RecvBuff[6];
                    tempUnion.C8Data[0]     = UART1RecvBuff[7];
                    //firResult               = FilterCalculation((double)tempUnion.I16Data, MagnXRawDataFIFO, FilterHn, FilterOrder);
                    //sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "P:%06d ", tempUnion.I16Data);
                    sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "P:%06d ", tempUnion.I16Data);
                    //tempUnion.I16Data=(int)firResult;
                    //pitch0=tempUnion.I16Data;
                    //firResult               = FilterCalculation((double)tempUnion.I16Data, MagnXRawDataFIFO, FilterHn, FilterOrder);
                    pitch_atti=tempUnion.I16Data;
                    pitch_deta=tempUnion.I16Data-pitch0; //當(dāng)前的差值

                    xiangying_pid=Kp_pitch*pitch_deta+Kd_pitch*(pitch_deta-pitch_deta_1)/time;//PID_xiangying
                    //下面code記錄上次數(shù)據(jù) （誤差和具體數(shù)值）
                    pitch0=tempUnion.I16Data;
                    pitch_deta_1=pitch_deta;//記錄當(dāng)前的差值，以便下次使用
                    UART1TransToUART2Size   = UART1TransToUART2Size + 9;
                    tempUnion.C8Data[1]     = UART1RecvBuff[8];
                    tempUnion.C8Data[0]     = UART1RecvBuff[9];
                    sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Y:%06d \r\n", xiangying_pid);
                    UART1TransToUART2Size   = UART1TransToUART2Size + 9 + 2;
                    break;
            }
        }
        osSemaphoreRelease(myBinarySem_UART1ToUART2TransReadyHandle);
        osDelay(time);
    }
  /* USER CODE END StartTaskUART1GY953 */
}

/* USER CODE BEGIN Header_StartTaskUART2 */
/**
* @brief Function implementing the myTask03 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTaskUART2 */
void StartTaskUART2(void const * argument)
{
  /* USER CODE BEGIN StartTaskUART2 */
    HAL_UART_Transmit_IT(&huart2, "Ready!", 6);  //ready對(duì)應(yīng) 52 65 61 64 79 21


  /* Infinite loop */
    for(;;)
    {
        osSemaphoreWait(myBinarySem_UART1ToUART2TransReadyHandle, osWaitForever);
        osSemaphoreWait(myBinarySem_UART2SendHandle, 100);
        memcpy(UART2SendBuff, UART1TransToUART2Buff, UART1TransToUART2Size);
        UART2SendSize   = UART1TransToUART2Size;
        HAL_UART_Transmit_IT(&huart2, UART2SendBuff, UART2SendSize);  //uart2將uart1收到的數(shù)據(jù)發(fā)到電腦
        osDelay(1);
    }
  /* USER CODE END StartTaskUART2 */
}

/* USER CODE BEGIN Header_StartTaskMotorCtrl */
/**
* @brief Function implementing the myTask04 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTaskMotorCtrl */
void StartTaskMotorCtrl(void const * argument)
{
    /* USER CODE BEGIN StartTaskMotorCtrl */
      HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
      HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
      /* Infinite loop */
//      uint8_t flag = 0;  //量程判斷
//      int16_t step = 500;  //步長
      uint8_t flag = 0;  //量程判斷
      int16_t speed_deta=30;

      for(;;)
      {
         if(time_delay<5000)
         {
                 if((pitch_atti<=-10||pitch_atti>800)||(xiangying_pid>8||xiangying_pid<-8))
                 {
                 
                     if(pitch_atti<=140)   //前期前進(jìn)
                      {
                          time_delay=0;
                          spdleft=-68;
                          spdright=-68;
                      }
                     else if(pitch_atti>=908){
                         time_delay=0;
                         spdleft=60;
                         spdright=80;
                     }
                     else    //上去突變之后后退
                      {
                          spdleft=0.3*xiangying_pid;
                          spdright=0.3*xiangying_pid;
                          if(spdleft<15&&spdright<50&&spdleft>=0&&spdright>=0)
                          {
                              time_delay=time_delay+time;
                              HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
                          }
                          if(spdleft<0&&spdleft>-50&&spdright>-50&&spdright<0)
                          {
                              time_delay=time_delay+time;
                              HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
                          }
                      }
                 }
                 else
                 {
                     spdleft=0;
                     spdright=0;
                     time_delay=time_delay+time;
                     HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
                 }
         }
         else
         {
             spdleft=80;
             spdright=100;
         }
//         if(deta_data>1500)
//         {
//          int16_t step = 100;  //步長
//          if (flag == 0) {
//            spdleft  =spdleft+step;
//            spdright =spdright+step;
//            if (spdleft >=1000) {
//              spdleft = 1000;
//              flag     = 1;
//            }
//            if (spdright >=1000) {
//              spdright = 1000;
//              flag     = 1;
//            }
//          } else {
//            spdleft  = spdleft-step;
//            spdright = spdright-step;
//            if (spdleft <= -1000) {
//              spdleft = -1000;
//              flag     = 0;
//            }
//            if (spdright <= -1000) {
//              spdright = -1000;
//              flag     = 0;
//            }
//          }
//         }
//        //osSemaphoreWait (myBinarySem_KeyPressHandle, osWaitForever);
        //HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
//        spdleft=15;
//        spdright=50;
        if(spdleft>0&&spdright>0)
        {

            MotorCtrl(0,spdleft+speed_deta);
            MotorCtrl(1,spdright);
        }
        else
        {
            MotorCtrl(0,spdleft);
            MotorCtrl(1,spdright);
        }
        //倒轉(zhuǎn) 0比1大30
        //正轉(zhuǎn) 0比1小8
        osDelay(time);
      }
      /* USER CODE END StartTaskMotorCtrl */
}

/**
  * @brief  Period elapsed callback in non blocking mode
  * @note   This function is called  when TIM1 interrupt took place, inside
  * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
  * a global variable "uwTick" used as application time base.
  * @param  htim : TIM handle
  * @retval None
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  /* USER CODE BEGIN Callback 0 */

  /* USER CODE END Callback 0 */
  if (htim->Instance == TIM1) {
    HAL_IncTick();
  }
  /* USER CODE BEGIN Callback 1 */

  /* USER CODE END Callback 1 */
}

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

到了這里，關(guān)于基于STM32-F401的平衡小車的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！