四足機器人控制系統(tǒng)設(shè)計及其仿真

機器人控制系統(tǒng)概述機器人控制系統(tǒng)研究的背景國內(nèi)外研究現(xiàn)狀和發(fā)展趨勢：隨著機器人技術(shù)的發(fā)展，機器人應(yīng)用領(lǐng)域的不斷擴大，對機器人的性能提出了更高的要求，因此，如何有效地將其他領(lǐng)域(如圖像處理、聲音識別、最優(yōu)控制人工智能等)的研究成果應(yīng)用到機器人控制系統(tǒng)的實時操作中，是一項富有挑戰(zhàn)性的研究工作。而具有開放式結(jié)構(gòu)的模塊化、標(biāo)準(zhǔn)化機器人，其控制系統(tǒng)的研究無疑對提高機器人性能和自主能力，推動機器人技術(shù)的發(fā)展具有重大意義。圖1-1四足機器人隨著機器人控制技術(shù)的發(fā)展，針對結(jié)構(gòu)封閉的機器人控制器的缺陷，開發(fā)“具有開放式結(jié)構(gòu)的模塊化、標(biāo)準(zhǔn)化機器人控制器”是當(dāng)前機器人控制器的一個發(fā)展方向。近幾年，日本、美國和歐洲一些國家都在開發(fā)具有開放式結(jié)構(gòu)的機器人控制器，如日本安川公司基于PC開發(fā)的具有開放式結(jié)構(gòu)、網(wǎng)絡(luò)功能的機器人控制器。我國863計劃智能機器人主題也已對這方面的研究立項。由于適用于機器人控制的軟、硬件種類繁多和現(xiàn)代技術(shù)的飛速發(fā)展，開發(fā)一個結(jié)構(gòu)完全開放的標(biāo)準(zhǔn)化機器人控制器存在一定困難，但應(yīng)用現(xiàn)有技術(shù)，如工業(yè)PC良好的開放性、安全性和互聯(lián)網(wǎng)相同性，標(biāo)準(zhǔn)的實時多任務(wù)操作系統(tǒng)，標(biāo)準(zhǔn)的總線結(jié)構(gòu)，標(biāo)準(zhǔn)接口等，打破現(xiàn)有機器人控制結(jié)構(gòu)封閉的局面，開發(fā)結(jié)構(gòu)開放性、功能模塊化的標(biāo)準(zhǔn)機器人控制器是完全可行的。機器人控制系統(tǒng)的特點(1)機器人控制系統(tǒng)是一個多變量控制系統(tǒng)，即使簡單的工業(yè)機器人也有3～5個自由度，比較復(fù)雜的機器人有十幾個自由度，甚至幾十個自由度，每個自由度一般包含一個伺服機構(gòu)多個獨立的伺服系統(tǒng)必須有機地協(xié)調(diào)起來。例如，機器人的手部運動是所有關(guān)節(jié)的合成運動，要使手部謝安照一定的軌跡運動就必須控制各關(guān)節(jié)協(xié)調(diào)運動，包括運動軌跡、動作時序等多方面的協(xié)調(diào)。(2)運動描述復(fù)雜，機器人的控制與機構(gòu)運動學(xué)及動力學(xué)密切相關(guān)。描述機器人狀態(tài)和運動的數(shù)學(xué)模型是一個非線性模型，隨著狀態(tài)的變化，其參數(shù)也在變化，各變量之間還存在耦合。因此，僅僅考慮位置閉環(huán)是不夠的，還要考慮速度閉環(huán)，甚至加速度閉環(huán)。在控制過程中，根據(jù)給定的任務(wù)，應(yīng)當(dāng)選擇不同的基準(zhǔn)坐標(biāo)系，并做適當(dāng)?shù)淖鴺?biāo)變換，求解機器人運動學(xué)正解和逆解。此外，還要考慮各關(guān)節(jié)之間慣性力、哥氏力等的耦合作用和重力負(fù)載的景影響，因此，系統(tǒng)中還經(jīng)常采用一些控制策略，如重力補償、前饋、解耦或自適應(yīng)控制等。(3)具有較高的重復(fù)定位精度，系統(tǒng)剛性好。除直角坐標(biāo)機器人外，機器人關(guān)節(jié)上的位置檢測元件不能安裝在未端執(zhí)行器上，而應(yīng)安裝在各自的驅(qū)動軸上，構(gòu)成位置半閉環(huán)系統(tǒng)。但機器人的重復(fù)定位精度較高，一般為±0.1mm。此外，由于機器人運行時要求運動平穩(wěn)，不受外力干擾，為此系統(tǒng)應(yīng)具有較好的剛性(4)信息運算量大。機器人在動作上往往可以通過不同的方式和路徑來完成，因此存在一個最優(yōu)的問題，較高級的機器人可以采用人工智能的方法;用計算機建立起龐大的信息庫，借助信息庫進行控制、決策管理和操作。根據(jù)傳感噐和模式識別的方法獲得對象及環(huán)境的工況，按照給定的指標(biāo)要求，自動選擇最佳的控制規(guī)律。(5)需采用加或減速控制。過大的加或減速度會景影響機器人運動的平穩(wěn)性，甚至使機器人發(fā)生抖動，因此在機器人起動或停止時采取加或減速控制策略。通常采用勻加或減速運動指令來實現(xiàn)。此外，機器人不允許有位置超調(diào)否則將可能與工件發(fā)生碰撞。因此，要求控制系統(tǒng)位置無超調(diào)，動態(tài)響應(yīng)盡量快?；趩纹瑱C四足機器人控制系統(tǒng)的優(yōu)點基于單片機機器人控制系統(tǒng)的優(yōu)點機器人控制系統(tǒng)無疑是對現(xiàn)代機器人在如何協(xié)調(diào)運行方面非常重要的系統(tǒng)，小型機器人的控制系統(tǒng)通常采用一些功能相對獨立的單片機系統(tǒng)來實現(xiàn)，對于用單片機作為小型機器人的控制系統(tǒng)的優(yōu)勢，它具多功能、高效率、高性能、低電壓、低功耗、低價格等優(yōu)點。而單片機中STM32系列具有低功耗、廉價、穩(wěn)定性能好、功能強大等優(yōu)點?；赟TM32單片機的四足機器人控制系統(tǒng)是一種對單片機性能要求較高，結(jié)構(gòu)相對簡單，實用性較強的中高端電子產(chǎn)品，以單片機作為核心器件，A/D轉(zhuǎn)換元件為重要部分，以其體積小、成本低、相應(yīng)速度快等優(yōu)點，其具有很好的使用意義?；趩纹瑱C機器人控制系統(tǒng)的缺點對于使用單片機作為機器人控制系統(tǒng)的小型機器人而言，小型機器人運動時所受到的干擾幅度的影響相較于大型機器人受到干擾幅度的影響相對較大，但由于以單片機作為控制系統(tǒng)，控制命令全由單片機輸出且單片機易受到干擾，所以導(dǎo)致機器人抗干擾能力差。在處理任務(wù)方面，單片機處理任務(wù)單一，運算速度較其他基礎(chǔ)的控制系統(tǒng)慢。基于STM32單片機四足機器人控制系統(tǒng)研發(fā)的可行性價格低廉系統(tǒng)中采用了簡單常見、安全可靠的外設(shè)，如電機和電池都是市面上常見的型號，價格穩(wěn)定且并不昂貴，且STM32單片機價格有相對便宜、功能強大和簡單操控等特點。方便安裝整個系統(tǒng)要方便安裝，這樣才能減少安裝成本，由于該控制系統(tǒng)的主要控制中樞為單片機，個頭小且結(jié)構(gòu)簡單，可以自己動手安裝，一方面可以獲得一些樂趣，另一方面如果出現(xiàn)故障問題，也易于排查。結(jié)構(gòu)簡單且方便安裝也可以讓該系統(tǒng)有更廣闊的市場需求，低廉的成本，實用的系統(tǒng)，這樣才能給系統(tǒng)的進一步完善提供條件。所以安裝方面是設(shè)計的一個優(yōu)點，由于控制系統(tǒng)與其他執(zhí)行元件連接緊湊，由單片機連接各元件的線路也相對清晰，易于安裝。市場性腿式機器人系統(tǒng)是履帶和輪式機器人的替代品，適用于崎嶇的地形和復(fù)雜的環(huán)境。機械腿與不同環(huán)境中選擇的接觸點的自由度使他們能夠克服與腿長相當(dāng)?shù)恼系K。有了這樣的功能，有腿的機器人有一天可以在森林和山脈中營救人員，爬樓梯以在建筑工地中攜帶有效載荷，檢查非結(jié)構(gòu)化的地下隧道并探索其它未知地域。腿式系統(tǒng)具有執(zhí)行人類和動物能夠進行的任何的運動潛力。研究分析基本思路研究的基本思路是以單片機為中心，單片機所在電路板為必要部分，再進行外圍的設(shè)計。主要的研究方法為設(shè)計電路圖，編程，進行仿真。技術(shù)路線先理清四足機器人控制系統(tǒng)的含義及意義，收集資料，分析資料，再通過學(xué)習(xí)吸取經(jīng)驗，進行設(shè)計與編程，檢驗方案的可行性，多次修改方案，最終達到最完美的效果。設(shè)計方案首先學(xué)習(xí)STM32單片機，掌握所以相關(guān)知識后，用KeiluVision5編寫程序，并與proteus聯(lián)用仿真，檢驗方案的可行性。四足機器人控制系統(tǒng)分析單片機簡介單片機是單片機微型計算機的簡稱，一種典型的嵌入式微控制器（MicrocontrollerUnit）。它是一種集成電路芯片，把具有數(shù)據(jù)處理能力的中央處理器CPU和一系列功能集成在一起的計算機系統(tǒng)。單片機分類單片機的分類有STM32系列單片機、51系列單片機、PIC單片機、AVR單片機。STM32單片機是專為要求高性能、低成本、低功耗的嵌入式應(yīng)用設(shè)計的單片機，功能強大且實用性廣，能完成相對復(fù)雜的需求。51單片機是目前最廣泛應(yīng)用的單片機之一，也是單片機的入門級單片機，易學(xué)且實用性廣。PIC單片機指令數(shù)量比較少，編寫程序較為麻煩。精簡指令使其執(zhí)行效率高，保密熔絲使其保密性好。AVR單片機是一種精簡指令集高速8位單片機，其主要特點是執(zhí)行速度快，可靠性高，低功耗。STM32F407簡介其中STM32F407是STM32F4系列單片機的一種，STM32F407提供了工作頻率為168MHz的Cortex?-M4內(nèi)核的性能。在168MHz頻率下，從Flash存儲器執(zhí)行時，STM32F407能夠提供210DMIPS/566CoreMark性能，并且利用意法半導(dǎo)體的ART加速器實現(xiàn)了FLASH零等待狀態(tài)。DSP指令和浮點單元擴大了產(chǎn)品的應(yīng)用范圍。該系列產(chǎn)品采用意法半導(dǎo)體90nm工藝和ART加速器，具有動態(tài)功耗調(diào)整功能，能夠在運行模式下和從Flash存儲器執(zhí)行時實現(xiàn)低至238μA/MHz的電流消耗。圖4-1STM32F407引腳圖Keil簡介Keil軟件是目前廣泛運用于單片機系列的軟件。Keil提供了包括C編譯器、宏匯編、鏈接器、庫管理和一個功能強大的仿真調(diào)試器等在內(nèi)的完整開發(fā)方案，通過一個集成開發(fā)環(huán)境將這些部分組合在一起。KeilC51生成的目標(biāo)代碼效率非常之高，多數(shù)語句生成的匯編代碼很緊湊，容易理解。在開發(fā)大型軟件時更能體現(xiàn)高級語言的優(yōu)勢。與匯編相比，Keil可使用的C語言是較為簡單易懂的，也是計算機語音入門的基礎(chǔ)，因此該軟件也易于上手。Keil使用流程如下：第一步：在菜單欄中點擊“project”選擇NewuVisionProject”新建一個工程，在彈出對話框中，工程文件選擇要放置的文件夾。彈出一個框，選擇在CPU類型如STM32F407系列芯片。選擇好后會彈出一個窗口提示是否添加啟動代碼。這樣新工程的創(chuàng)建完畢。第二步：點擊工具欄，選擇Filenew，新建一個空白文檔。在空白處寫入程序。單擊左上方的Save，保存好源程序，并把文檔后綴定為“.C”。如果是匯編語言，后綴為“.ASM”。把剛創(chuàng)建的源程序文件加入到工程項目文件中，右擊右方“Sourcegroup”，單點“ADD”按鈕時，選擇剛保存的源程序文件，單擊“Close”。第三步：單擊Project,選擇“optionsforTarget（自定義）”,在“xtal（MHZ）”中設(shè)置要設(shè)置的晶振參數(shù)。單擊Output欄，在“CreateHEXFile”前面打勾，這樣做是為了輸出hex文件，用于程序在仿真軟件proteus上調(diào)試。第四步：單擊左上方的Build，進行編譯。第五步：根據(jù)編譯后的反饋得知程序是否出錯，通過顯示錯誤代碼位置進行修改，直至編譯完全正確后，輸出的hex文件可直接使用。proteus簡介Proteus是一種EDA工具軟件，它能仿真單片機、ARM等芯片及外圍器件，它能進行原理圖設(shè)計、代碼的調(diào)試、PCB設(shè)計并能與KEIL聯(lián)動調(diào)試，是國內(nèi)外著名的EDA仿真軟件。Proteus基本操作：元件的選擇：按界面的左上方P按鈕可彈出選擇元件框。選擇要使用的元件：在PickDevice彈出窗口Keywords中輸入關(guān)鍵字。然后選中所需元件、單擊OK。放置元件到繪圖區(qū)：單擊工作界面左邊的列表元件，然后在右側(cè)的繪圖區(qū)單擊選中所需放置的位置。移動元件位置：右擊選中元件，然后按住左鍵進行拖動。刪除元件：右擊選中元件使元件呈紅色，選中后再一次右擊則是刪除，也可以在右擊后選擇“DeletetObject”旋轉(zhuǎn)元件：右擊選中元件，在彈出窗口的旋轉(zhuǎn)工具欄選擇旋轉(zhuǎn)方式。元件連線：在鼠標(biāo)指針處于筆狀時，左擊線的初始位置，再左擊選擇結(jié)束位置。電源和地的繪制：單擊工具欄上的TerminalsMode，左側(cè)工具欄顯示TERMINALS時，可在其中選擇POWER或GROUND等符合，在放置到繪圖區(qū)。仿真開始：單擊左下方的Play按鍵，即開始按鈕進行原理圖仿真測試。仿真結(jié)束：單擊左下方Stop按鍵，結(jié)束仿真?；赟TM32單片機的四足機器人控制系統(tǒng)的硬件設(shè)計在本設(shè)計中基于STM32單片機作為控制系統(tǒng)的設(shè)計以對各元件運行控制為基礎(chǔ)，對硬件設(shè)計為輔，達到控制系統(tǒng)可以完整控制各個硬件運作。四足機器人控制系統(tǒng)的核心為STM32F407單片機。主要電路結(jié)構(gòu)圖4-1主要電路結(jié)構(gòu)示意圖電源電源使用DJITB470D型號的電池作為四足機器人的主要電源，該型號的電池能提供24V的直流電，選擇該直流電源能保證電路供應(yīng)電流穩(wěn)定。分電板圖4-2分電板主要電路結(jié)構(gòu)圖中PDB為分電板，可將電池的電流分流到多個需要使用的接口，供多個接口一起使用電池供應(yīng)的穩(wěn)定電流，接口段為塑料接口，可達到減輕機器人整體重量的目的。陀螺儀圖4-3陀螺儀電路結(jié)構(gòu)示意圖中的IMU為陀螺儀，它是一個角度傳感器，即慣性測量單元，它由三個單軸的加速度計和三個單軸的陀螺儀組成，加速度計檢測物體在載體坐標(biāo)系統(tǒng)獨立三軸的加速度信號，而陀螺儀檢測載體相對于導(dǎo)航坐標(biāo)系的角速度信號，對這些信號進行處理之后，便可解算出物體的姿態(tài)，通過陀螺儀中標(biāo)定的坐標(biāo)軸yaw軸pitch軸roll軸，解算出當(dāng)前所處在的運動位置的偏差，為四足機器人運動時產(chǎn)生的偏差進行校正提供依據(jù)。本次設(shè)計使用的為IMUICM-206026-axis型號的陀螺儀，軸加速計噪聲100ug/Hz。電調(diào)圖4-4電調(diào)電調(diào)，全稱電子調(diào)速器，它的輸入線可連接電源，輸出線可連接電機。在電路結(jié)構(gòu)圖中為Encoder，本次設(shè)計中使用的是C620型號的電調(diào)輸入線連接STM32單片機，由單片機進行控制它的電輸入，達到根據(jù)控制信號調(diào)節(jié)電動機的轉(zhuǎn)速的目的。電機在電路結(jié)構(gòu)圖中MOTO為電機，本設(shè)計的電機使用3508電機，在每條腿上各有兩個電機分別驅(qū)動組成一條腿的兩個連軸，以達到控制一條腿進行運動的目的。程序部分編程 圖5-1總體流程圖整個程序編程部分如上總體流程圖所示，首先要對四足機器人的路徑規(guī)劃、落腳點選擇以及運動指令的生成要有初步成型的預(yù)計，為之后各執(zhí)行過程中所要達到的效果做指引。在對四足機器人的運動時大致要遇到的各種復(fù)雜情況和對機械腿的落腳范圍做預(yù)估后，要對控制系統(tǒng)可通過外部控制信號對四足機器人運動執(zhí)行進行控制的方式進行設(shè)置。在設(shè)置完機械腿的運動期望和落腳點信息后，在啟動時首先要根據(jù)陀螺儀對當(dāng)前四足機器人整體姿態(tài)進行定位，通過對于定位時通過陀螺儀產(chǎn)生的坐標(biāo)來判定是否偏移目標(biāo)平穩(wěn)坐標(biāo)，若檢測有偏差后便將偏差反饋到原始設(shè)定的各機械腿的運動軌跡方程與之對比后可進行修正。待修正完后，便可通過芯片對相應(yīng)腿部的電機進行按照預(yù)定運動軌跡方程運行控制，達到生成指定步態(tài)的效果。在機械腿上的運動效果也可通過控制器去反饋機械能否按照預(yù)期運行的信息，從而達到對四足機器人遇到不同情況進行不同的運動軌跡反應(yīng)。遙控接收處理函數(shù)圖5-2遙控接收處理函數(shù)該程序通過Usart2開啟DMA傳輸，非阻塞接收遙控發(fā)過來的數(shù)據(jù)，接收完數(shù)據(jù)在串口空閑中斷進行數(shù)據(jù)解析，不影響其它運算量大的任務(wù)，做到效率最大化。代碼實例：#define

RECEIVELEN

128

//一次DMA最大接受128字節(jié)

uint8_t

rc_RxBuffer[RECEIVELEN];//DMA接收緩存數(shù)組

//遙控器控制變量

static

RC_Ctrl_t

RC_CtrlData

=

{0};

static

uint16_t

temp;//儲存DMA剩余傳輸數(shù)據(jù)位數(shù)

//初始化DMA，串口2

void

remote_control_init(void)

{

RC_Init(rc_RxBuffer,RECEIVELEN);

}

//返回遙控器控制變量，通過指針傳遞方式傳遞信息

const

RC_Ctrl_t

*get_remote_control_point(void)

{

return

&RC_CtrlData;

}

void

USART2_IRQHandler(void)

{

if

(__HAL_UART_GET_FLAG(&huart2,

UART_FLAG_IDLE)

&&

__HAL_UART_GET_IT_SOURCE(&huart2,

UART_IT_IDLE))

{

uint16_t

tmp

=

huart2.Instance->SR;

tmp

=

huart2.Instance->DR;

tmp--;

__HAL_UART_CLEAR_IDLEFLAG(&huart2);

CLEAR_BIT(huart2.Instance->SR,

USART_SR_IDLE);

__HAL_DMA_DISABLE(huart2.hdmarx);

temp

=

huart2.hdmarx->Instance->NDTR;

if((RECEIVELEN

-

temp)

==

18)//判斷已接受DMA數(shù)據(jù)多少位

{

DetectHook(0);

Get_Remote_info(&RC_CtrlData

,rc_RxBuffer);

}

HAL_UART_Receive_DMA(&huart2,

(uint8_t

*)rc_RxBuffer,

RECEIVELEN);

SET_BIT(huart2.Instance->CR1,

USART_CR1_IDLEIE);

DMA1->HIFCR

=

DMA_FLAG_DMEIF0_4

|

DMA_FLAG_FEIF0_4

|

DMA_FLAG_HTIF0_4

|

DMA_FLAG_TCIF0_4

|

DMA_FLAG_TEIF0_4;

__HAL_DMA_SET_COUNTER(huart2.hdmarx,

RECEIVELEN);

__HAL_DMA_ENABLE(huart2.hdmarx);

}

}

void

Get_Remote_info(RC_Ctrl_t

*rc_ctrl

,volatile

const

uint8_t

*sbus_buf)

{

rc_ctrl->rc.ch[0]

=

(sbus_buf[0]

|

(sbus_buf[1]

<<

8))

&

0x07ff;

//!<

Channel

0

rc_ctrl->rc.ch[1]

=

((sbus_buf[1]

>>

3)

|

(sbus_buf[2]

<<

5))

&

0x07ff;

//!<

Channel

1

rc_ctrl->rc.ch[2]

=

((sbus_buf[2]

>>

6)

|

(sbus_buf[3]

<<

2)

|

//!<

Channel

2

(sbus_buf[4]

<<

10))

&0x07ff;

rc_ctrl->rc.ch[3]

=

((sbus_buf[4]

>>

1)

|

(sbus_buf[5]

<<

7))

&

0x07ff;

//!<

Channel

3

rc_ctrl->rc.s[0]

=

((sbus_buf[5]

>>

4)

&

0x0003);

//!<

Switch

left

rc_ctrl->rc.s[1]

=

((sbus_buf[5]

>>

4)

&

0x000C)

>>

2;

//!<

Switch

right

rc_ctrl->mouse.x

=

sbus_buf[6]

|

(sbus_buf[7]

<<

8);

//!<

Mouse

X

axis

rc_ctrl->mouse.y

=

sbus_buf[8]

|

(sbus_buf[9]

<<

8);

//!<

Mouse

Y

axis

rc_ctrl->mouse.z

=

sbus_buf[10]

|

(sbus_buf[11]

<<

8);

//!<

Mouse

Z

axis

rc_ctrl->mouse.press_l

=

sbus_buf[12];

//!<

Mouse

Left

Is

Press

?

rc_ctrl->mouse.press_r

=

sbus_buf[13];

//!<

Mouse

Right

Is

Press

?

rc_ctrl->key.v

=

sbus_buf[14]

|

(sbus_buf[15]

<<

8);

//!<

KeyBoard

value

rc_ctrl->rc.ch[4]

=

sbus_buf[16]

|

(sbus_buf[17]

<<

8);

//NULL

rc_ctrl->rc.ch[0]

-=

RC_CH_VALUE_OFFSET;

rc_ctrl->rc.ch[1]

-=

RC_CH_VALUE_OFFSET;

rc_ctrl->rc.ch[2]

-=

RC_CH_VALUE_OFFSET;

rc_ctrl->rc.ch[3]

-=

RC_CH_VALUE_OFFSET;

rc_ctrl->rc.ch[4]

-=

RC_CH_VALUE_OFFSET;

}

}電機與陀螺儀數(shù)據(jù)接收發(fā)送函數(shù)圖5-3電機與陀螺儀接收發(fā)送函數(shù)編程電機與陀螺儀接收發(fā)送函數(shù)后，使單片機透過陀螺儀的三個定位基準(zhǔn)軸給與的定位，對電機進行控制與調(diào)整機器人。代碼實例：/********************************************************************/*********************陀螺儀數(shù)據(jù)解析儲存結(jié)構(gòu)體**********************//陀螺儀數(shù)據(jù)結(jié)構(gòu)體

typedef

struct

{

float

v_x;//X軸速度

float

v_z;//Z軸速度

double

pit;//PIT軸角度

double

yaw;//YAW軸角度

float

acc_x;//X軸加速度

float

acc_z;//Z軸加速度

float

acc_y;//Y軸加速度

double

yaw_init_offset;

int16_t

raw_pit;

int16_t

raw_yaw;

}Gyro_info_t;

/********************************************************************//********************************************************************/*********************電機數(shù)據(jù)解析儲存結(jié)構(gòu)體**********************//rm電機統(tǒng)一數(shù)據(jù)結(jié)構(gòu)體，電機通過CAN通信與主控連接，主控接收到完整一次數(shù)據(jù)進入Can中斷處理，解析typedef

struct

{

uint16_t

ecd;//編碼值

uint16_t

send_ecd;

int16_t

speed_rpm;//轉(zhuǎn)速

int16_t

given_current;//輸出電流

uint8_t

temperate;//溫度

int16_t

last_ecd;//上次編碼值

fp32

total_angle;//總角度

uint16_t

offset_ecd;//偏移編碼值

int32_t

round_cnt;//電機圈數(shù)

}

motor_measure_t;

//陀螺儀數(shù)據(jù)結(jié)構(gòu)體

/********************************************************************///電機數(shù)據(jù)讀取解析函數(shù)

void

get_motor_measure(motor_measure_t*

ptr,

uint8_t

Data[]){詳細內(nèi)容查看附錄}//電機發(fā)送函數(shù)void

Send_3508_Id1_Id4(int16_t

a,

int16_t

b,

int16_t

c,

int16_t

d)

{詳細內(nèi)容查看附錄}姿態(tài)控制該程序可將機器腿上兩個連軸杠的預(yù)期轉(zhuǎn)動角度變化轉(zhuǎn)化為對電機運行的控制，控制電機轉(zhuǎn)動使得兩連軸杠能轉(zhuǎn)動到相應(yīng)角度的位置，達到腿能運行的目的。腿通過角度環(huán)控制八個電機，實現(xiàn)腿的開環(huán)軌跡從而行走。主要通過改變步態(tài)的參數(shù)，如頻率，占空比等，正弦的幾何參數(shù)或峰值等，軌跡坐標(biāo)轉(zhuǎn)換為所需腿角度θ即菱形腿中線相對于縱軸偏移角度和對稱關(guān)節(jié)夾角度一半，然后由這兩個角度算出上關(guān)節(jié)所需運動的角度theta1和theta2從而根據(jù)實際電機傳動結(jié)構(gòu)得出電機所需轉(zhuǎn)動角度，通過角度環(huán)精準(zhǔn)到位。角度環(huán)比例項增加了腿的剛度，微分項則抵消了關(guān)節(jié)的阻尼。圖5-4腿部角度坐標(biāo)簡圖圖5-5腿部擺動角度坐標(biāo)簡圖運動軌跡解算：圖5-6運動軌跡解算流程圖主要算法如下：圖5-7主要算法圖5-8主要算法圖5-9主要算法檢驗步態(tài)參數(shù)設(shè)置是否合理，其主要判斷最低高度，是否超過腿長，最高高度是否達到機械限位以及步態(tài)頻率過低過高，還有占空比。圖5-10檢驗步態(tài)參數(shù)合理代碼步行步態(tài)以下便是步態(tài)結(jié)構(gòu)體參數(shù)（步行）：{stance_height,down_AMP,up_AMP,flight_percent(proportion),step_length,FREQ}{0.24,0.025f,0.09,0.35,0.09,1.5f,0.0},//TROTStance_height：正常站立高度down_AMP：下降最大高度up_AMP：上升最大高度flight_percent：腿在空中占整個運動時間step_length：前進方向步移FREQ:該步態(tài)頻率停下以下便是步態(tài)結(jié)構(gòu)體參數(shù)（步行）{stance_height,down_AMP,up_AMP,flight_percent(proportion),step_length,FREQ}{0.22,0.0,0.0,0.35,0.0,1.0,0.0},//STOPStance_height：正常站立高度down_AMP：下降最大高度up_AMP：上升最大高度flight_percent：腿在空中占整個運動時間step_length：前進方向步移FREQ:該步態(tài)頻率原地跳躍圖5-11跳躍步驟流程圖當(dāng)遙控切換到跳躍擋，斜坡參數(shù)初始化（避免動作過快），腿先壓縮使整只狗降到最低，然后全力向上，達到跳躍目的。圖5-12原地跳躍代碼圖5-13原地跳躍代碼使用proteus進行系統(tǒng)的調(diào)試仿真的注意事項Proteus中的電路圖基本與硬件原理圖相同，主要注意的地方有：第1點是在proteus中的文件庫沒有部分元件時，可采用小燈泡來代替，若小燈泡按照規(guī)定閃爍，則證明該小燈泡代替的元件能正常運行。第2點是本次仿真所使用的proteus仿真軟件為8.0版本，所以最高支持的芯片為STM32F1系列芯片，所以在低版本的條件下將減少一條機械腿的仿真，但由于每條對角線的兩條腿運動時候的狀態(tài)相同，可以參考對角線那條腿的運動仿真情況判斷程序是否出問題。圖6-1關(guān)于機械腿電機運行情況模擬仿真圖Keil與proteus聯(lián)調(diào)先在keil上把程序調(diào)試成功并生成HEX文件，然后在proteus上單擊單片機，在彈出窗口中ProgramFile一欄選擇生成的HEX文件并載入單片機中，然后開始調(diào)試。調(diào)試方法通過相應(yīng)元件對應(yīng)的引腳是否點亮可判斷出單片機是否按照目標(biāo)代碼運行，如果未點亮則是代碼里對應(yīng)引腳編碼錯誤，可通過keil5找到相應(yīng)位置進行修改。若相應(yīng)元件不按照規(guī)定運行，則是設(shè)定元件運行的代碼出錯，再返回原代碼修改運動時間參數(shù)進行調(diào)整。結(jié)論通過本次設(shè)計得出四足機器人控制系統(tǒng)由STM32單片機作為控制系統(tǒng)核心是可行的，該控制系統(tǒng)將編寫好的代碼如實將機械腿的轉(zhuǎn)動角度轉(zhuǎn)換成電信號輸出，對電機、電調(diào)等模塊進行控制?；赟TM32單片機的四足機器人控制系統(tǒng)是對單片機有一定性能要求，結(jié)構(gòu)相對簡單，實用性強的中高端電子產(chǎn)品，又由于單片機作為核心器件，在能很大程度上減小體積，降低成本且運行速度較快這些特點，其具有很好的意義。四足機器人與傳統(tǒng)的輪式和履帶式機器人相比，其靈活性更高，又由于是用機械腿行走，能適應(yīng)多種復(fù)雜地形，也可用于各類四足動物行為的模擬，這能發(fā)揮出該四足的體型能做出各種獨特動作的優(yōu)勢。參考文獻范永譚民.機器人控制器的現(xiàn)狀及發(fā)展.《機器人》，1999.孟冠彤.淺析機器人的發(fā)展進程以及機器人控制-《絲路視野》-2018.潘言全.多路電器遙控器的研究-《黑龍江科技信息》-2014.潘煉東.開放式機器人控制器及相關(guān)技術(shù)研究-《中國博士學(xué)位論文全文數(shù)據(jù)庫》-2007.毛鵬軍黃石生等.弧焊機器人焊縫跟蹤系統(tǒng)研究現(xiàn)狀及發(fā)展趨勢-《電焊機》-2001.夏如迪唐菲施吉瑞.電子圍欄低壓告警模塊的軟件設(shè)計實現(xiàn)-《數(shù)字技術(shù)與應(yīng)用》-2015.張帥帥.復(fù)雜地形環(huán)境中四足機器人行走方法研究-《中國博士學(xué)位論文全文數(shù)據(jù)庫》-2016.石美玉周欣榮.水輪機修復(fù)專用機器人本體測控系統(tǒng)開發(fā)研究-《黑龍江工程學(xué)院學(xué)報》-2004.周壽明1鄧成良2.可用于生產(chǎn)線的工業(yè)機器人研究-《科技創(chuàng)新導(dǎo)報》-2008.許東來張紹立余躍慶.基于柔性機器人的集成控制融合實驗研究探討-《微計算機信息》-2006.朱彥齊陳玉芝.淺談工業(yè)機器人在自動化控制領(lǐng)域的應(yīng)用-《職業(yè)》-2010.高向東丁度坤趙傳敏.機器視覺型焊縫跟蹤技術(shù)-《焊接》-2006.江天寶宋小平.試論基于PLC控制的機械手設(shè)計-《廣東科技》-2015.黃一平.基于運動模體機器人的軟件編程應(yīng)用-《信息技術(shù)與信息化》-2015.C51單片機的開發(fā)與應(yīng)用高銘澤-《硅谷》-2011.焦鉻戴小新.基于Proteus和Keil的單片機虛擬實驗室的構(gòu)建-《電腦知識與技術(shù)》-2010李衛(wèi)強董良雄嚴(yán)聞奇楊雨濱.基于小型移動設(shè)備的機艙數(shù)據(jù)采集輔助系統(tǒng)[J].機電工程,2015(09):121-124.王偉.基于PROTEUS與KEILC51軟件平臺的單片機仿真教學(xué)[J].電子制作,2015(13):96-96.高銘澤.C51單片機的開發(fā)與應(yīng)用[J].硅谷,2011(23):87-88.葉強戴建松高杉高潤.基于微機電系統(tǒng)運動傳感器在人體運動測量中的應(yīng)用[J].中國組織工程研究,2012(09):193-198.孫戴魏.淺議單片機原理及其信號干擾處理措施[J].企業(yè)導(dǎo)報,2012(03):296-297.焦鉻戴小新.基于Proteus和Keil的單片機虛擬實驗室的構(gòu)建[J].電腦知識與技術(shù),2010(28):257-258.孔旭.仿人關(guān)節(jié)型機器人創(chuàng)新平臺設(shè)計與開發(fā)[J].信息與電腦(理論版),2011(03):57-57吳泳.無人機多傳感器的集成與同步控制[J].自動化應(yīng)用,2012(08):71-73.李杰胡旭東趙勻.開放式結(jié)構(gòu)平臺上PUMA560機器人控制系統(tǒng)的研究[J].機械設(shè)計與研究,2002(05):19-21.附錄1遙控接收處理函數(shù)#include"remote_control.h"#include"stm32f4xx.h"#include"rc.h"#include"Detect_Task.h"#defineRECEIVELEN128uint8_trc_RxBuffer[RECEIVELEN];//遙控器控制變量staticRC_Ctrl_tRC_CtrlData={0};staticuint16_t temp;//初始化DMA，串口2voidremote_control_init(void){RC_Init(rc_RxBuffer,RECEIVELEN);}//返回遙控器控制變量，通過指針傳遞方式傳遞信息constRC_Ctrl_t*get_remote_control_point(void){return&RC_CtrlData;}voidUSART2_IRQHandler(void){ if(__HAL_UART_GET_FLAG(&huart2,UART_FLAG_IDLE)&& __HAL_UART_GET_IT_SOURCE(&huart2,UART_IT_IDLE)) { uint16_ttmp=huart2.Instance->SR;tmp=huart2.Instance->DR;tmp--; __HAL_UART_CLEAR_IDLEFLAG(&huart2); CLEAR_BIT(huart2.Instance->SR,USART_SR_IDLE); __HAL_DMA_DISABLE(huart2.hdmarx); temp=huart2.hdmarx->Instance->NDTR; if((RECEIVELEN-temp)==18) { DetectHook(0); Get_Remote_info(&RC_CtrlData,rc_RxBuffer); } HAL_UART_Receive_DMA(&huart2,(uint8_t*)rc_RxBuffer,RECEIVELEN); SET_BIT(huart2.Instance->CR1,USART_CR1_IDLEIE); DMA1->HIFCR=DMA_FLAG_DMEIF0_4|DMA_FLAG_FEIF0_4|DMA_FLAG_HTIF0_4|DMA_FLAG_TCIF0_4|DMA_FLAG_TEIF0_4; __HAL_DMA_SET_COUNTER(huart2.hdmarx,RECEIVELEN); __HAL_DMA_ENABLE(huart2.hdmarx); }}voidGet_Remote_info(RC_Ctrl_t*rc_ctrl,volatileconstuint8_t*sbus_buf){rc_ctrl->rc.ch[0]=(sbus_buf[0]|(sbus_buf[1]<<8))&0x07ff;//!<Channel0rc_ctrl->rc.ch[1]=((sbus_buf[1]>>3)|(sbus_buf[2]<<5))&0x07ff;//!<Channel1rc_ctrl->rc.ch[2]=((sbus_buf[2]>>6)|(sbus_buf[3]<<2)|//!<Channel2(sbus_buf[4]<<10))&0x07ff;rc_ctrl->rc.ch[3]=((sbus_buf[4]>>1)|(sbus_buf[5]<<7))&0x07ff;//!<Channel3rc_ctrl->rc.s[0]=((sbus_buf[5]>>4)&0x0003);//!<Switchleftrc_ctrl->rc.s[1]=((sbus_buf[5]>>4)&0x000C)>>2;//!<Switchrightrc_ctrl->mouse.x=sbus_buf[6]|(sbus_buf[7]<<8);//!<MouseXaxisrc_ctrl->mouse.y=sbus_buf[8]|(sbus_buf[9]<<8);//!<MouseYaxisrc_ctrl->mouse.z=sbus_buf[10]|(sbus_buf[11]<<8);//!<MouseZaxisrc_ctrl->mouse.press_l=sbus_buf[12];//!<MouseLeftIsPress?rc_ctrl->mouse.press_r=sbus_buf[13];//!<MouseRightIsPress?rc_ctrl->key.v=sbus_buf[14]|(sbus_buf[15]<<8);//!<KeyBoardvaluerc_ctrl->rc.ch[4]=sbus_buf[16]|(sbus_buf[17]<<8);//NULLrc_ctrl->rc.ch[0]-=RC_CH_VALUE_OFFSET;rc_ctrl->rc.ch[1]-=RC_CH_VALUE_OFFSET;rc_ctrl->rc.ch[2]-=RC_CH_VALUE_OFFSET;rc_ctrl->rc.ch[3]-=RC_CH_VALUE_OFFSET;rc_ctrl->rc.ch[4]-=RC_CH_VALUE_OFFSET;}附錄2電機與陀螺儀數(shù)據(jù)接收發(fā)送函數(shù)#include"CAN_Receive.h"#include"user_lib.h"#include"stm32f4xx.h"#include"FreeRTOS.h"#include"task.h"#include"can.h"#include"detect_task.h"#include"remote_control.h"#include"position_control.h"#include"arm_math.h"Gyro_info_tGyro_info;externStates_estate;//聲明電機變量staticmotor_measure_tLeg_motor[8];//底盤電機數(shù)據(jù)讀取voidget_motor_measure(motor_measure_t*ptr,uint8_tData[]){static int32_ttotal_ecd;if(state==INIT){ptr->offset_ecd=(uint16_t)(Data[0]<<8|Data[1]);ptr->round_cnt=0;}// (ptr)->last_ecd=(ptr)->ecd;// (ptr)->ecd=(uint16_t)(Data[0]<<8|Data[1]);(ptr)->speed_rpm=(uint16_t)(Data[2]<<8|Data[3]);(ptr)->speed_rpm/=15.4f; (ptr)->given_current=(uint16_t)(Data[4]<<8|Data[5]); (ptr)->temperate=Data[6];ptr->last_ecd=ptr->ecd;ptr->ecd=(uint16_t)(Data[0]<<8|Data[1]);if(ptr->ecd-ptr->last_ecd>6666){ptr->round_cnt--;}elseif(ptr->ecd-ptr->last_ecd<-6666){ptr->round_cnt++;}total_ecd=ptr->round_cnt*8192+ptr->ecd-ptr->offset_ecd;/*totalangle,unitisrad*///ptr->total_angle=loop_fp32_constrain(total_ecd/(8192.0f/(2*PI))/15.4f,-PI,PI); if(ptr==&Leg_motor[0]){ ptr->total_angle=loop_fp32_constrain((total_ecd/(8192.0f/(2*PI))/15.4f)-0.92f,-PI,PI); } elseif(ptr==&Leg_motor[1]){ ptr->total_angle=loop_fp32_constrain((total_ecd/(8192.0f/(2*PI))/15.4f)+2.7f,-PI,PI); if(ptr->total_angle>2.0f){ ptr->total_angle=ptr->total_angle-2*PI; } } elseif(ptr==&Leg_motor[4]){ ptr->total_angle=loop_fp32_constrain((total_ecd/(8192.0f/(2*PI))/15.4f)+0.84f,-PI,PI); } elseif(ptr==&Leg_motor[5]){ ptr->total_angle=loop_fp32_constrain((total_ecd/(8192.0f/(2*PI))/15.4f)-2.80f,-PI,PI); if(ptr->total_angle<-2.0f){ ptr->total_angle=ptr->total_angle+2*PI; } } elseif(ptr==&Leg_motor[2]){ ptr->total_angle=loop_fp32_constrain((total_ecd/(8192.0f/(2*PI))/15.4f)-2.69f,-PI,PI); if(ptr->total_angle<-2.0f){ ptr->total_angle=ptr->total_angle+2*PI; } } elseif(ptr==&Leg_motor[3]){ ptr->total_angle=loop_fp32_constrain((total_ecd/(8192.0f/(2*PI))/15.4f)+1.0f,-PI,PI); } elseif(ptr==&Leg_motor[6]){ ptr->total_angle=loop_fp32_constrain((total_ecd/(8192.0f/(2*PI))/15.4f)+2.8f,-PI,PI); if(ptr->total_angle>2.0f){ ptr->total_angle=ptr->total_angle-2*PI; } } elseif(ptr==&Leg_motor[7]){ ptr->total_angle=loop_fp32_constrain((total_ecd/(8192.0f/(2*PI))/15.4f)-0.93f,-PI,PI); }}//返回陀螺儀變量地址，通過指針方式獲取原始數(shù)據(jù)constGyro_info_t*get_GYRO_Measure_Point(void){ return&Gyro_info;}//返回腿部電機變量地址，通過指針方式獲得原始數(shù)據(jù)constmotor_measure_t*get_Leg_moto_Measure_Point(uint8_ti){ return&Leg_motor[i];}voidHAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef*hcan){ staticuint8_tDATA[8];//儲存CAN1接收數(shù)據(jù) staticuint8_tData[8];//儲存CAN2接收數(shù)據(jù) if(hcan==&hcan1) { //CAN1接收 HAL_CAN_GetRxMessage(&hcan1,CAN_RX_FIFO0,&Rx1Message,DATA); switch(Rx1Message.StdId) {staticuint8_ti=0; caseCAN_3508_M1_ID: caseCAN_3508_M2_ID: caseCAN_3508_M3_ID: caseCAN_3508_M4_ID:{ i=Rx1Message.StdId-CAN_3508_M1_ID; get_motor_measure(&Leg_motor[i],DATA); }break; default: { }break; } } elseif(hcan==&hcan2) { //CAN2接收 HAL_CAN_GetRxMessage(&hcan2,CAN_RX_FIFO0,&Rx2Message,Data); switch(Rx2Message.StdId) { case101: { DetectHook(1);//注冊時間，判斷是否陀螺儀掉線 staticint32_tyaw_connt=0; staticint16_traw_v_x,raw_v_z; staticfloatpitch_angle,yaw_angle,last_yaw_angle=0; raw_v_x=Data[0]<<8|Data[1]; raw_v_z=Data[2]<<8|Data[3]; Gyro_info.raw_pit=Data[4]<<8|Data[5]; Gyro_info.raw_yaw=Data[6]<<8|Data[7]; //陀螺儀原始數(shù)據(jù)為弧度，把弧度轉(zhuǎn)換為角度 Gyro_info.v_x=-(float)raw_v_x*0.057295f;//pit_Speed Gyro_info.v_z=(float)raw_v_z*0.057295f;//YAW_Speed //陀螺儀原始數(shù)據(jù)被乘100倍 pitch_angle=(float)Gyro_info.raw_pit/100.0f; yaw_angle=(float)Gyro_info.raw_yaw/100.0f; Gyro_info.pit=pitch_angle; if(Gyro_info.pit<-300) { Gyro_info.pit+=360; } elseif(Gyro_info.pit>300) { Gyro_info.pit-=360; } if((yaw_angle-last_yaw_angle)>330) { yaw_connt--; } elseif((yaw_angle-last_yaw_angle)<-330) { yaw_connt++; } Gyro_info.yaw=yaw_angle+yaw_connt*360; last_yaw_angle=yaw_angle;// printf("-%d-",HAL_GetTick()); }break; caseCAN_3508_M5_ID: caseCAN_3508_M6_ID: caseCAN_3508_M7_ID: caseCAN_3508_M8_ID:{ staticuint8_tii=0; ii=Rx2Message.StdId-CAN_3508_M1_ID; get_motor_measure(&Leg_motor[ii],Data); }break; default: { }break; } }}voidSend_3508_Id1_Id4(int16_ta,int16_tb,int16_tc,int16_td){ uint8_ti[8]; Tx1Message.RTR=CAN_RTR_DATA; Tx1Message.IDE=CAN_ID_STD; Tx1Message.StdId=0x200;Tx1Message.DLC=0x08; i[0]=a>>8;i[1]=a;i[2]=b>>8;i[3]=b; i[4]=c>>8;i[5]=c;i[6]=d>>8;i[7]=d; HAL_CAN_AddTxMessage(&hcan1,&Tx1Message,i,(uint32_t*)CAN_TX_MAILBOX0);}voidSend_3508_Id5_Id8(int16_ta,int16_tb,int16_tc,int16_td){uint8_ti[8];Tx2Message.RTR=CAN_RTR_DATA;Tx2Message.IDE=CAN_ID_STD;Tx2Message.StdId=0x1ff;Tx2Message.DLC=0x08; i[0]=a>>8;i[1]=a;i[2]=b>>8;i[3]=b; i[4]=c>>8;i[5]=c;i[6]=d>>8;i[7]=d; HAL_CAN_AddTxMessage(&hcan2,&Tx2Message,i,(uint32_t*)CAN_TX_MAILBOX0);}voidSend_Gyro_jiaozhun(uint8_tmode,uint16_ttime)//{ staticuint8_tData[8]; Tx2Message.StdId=100; Tx2Message.IDE=CAN_ID_STD; Tx2Message.RTR=CAN_RTR_DATA; Tx2Message.DLC=3; Data[0]=mode; Data[1]=time>>8; Data[2]=time; HAL_CAN_AddTxMessage(&hcan2,&Tx2Message,Data,(uint32_t*)CAN_TX_MAILBOX0);}附錄3角度轉(zhuǎn)化#include"math.h"#include"can_receive.h"#include"Position_Control.h"#include"jump.h"#include"REST.h"#include"arm_math.h"#include"stm32f4xx_hal.h"#include"NI_MING.h"#include"FreeRTOS.h"#include"task.h"#include"3508_drive.h"#include"remote_control.h"#include"detect_task.h"#include"user_lib.h"#include"ADC1.h"#include"WT931.h"#include"Hop.h"#include"jacobian.h"#include"dwt_stm32_delay.h"externramp_tstate_change_ramp;#defineL10.11f//upperleglength(m)#defineL20.19f//lowerleglength(m)//voidMOTO_ANGLE_TO_X_Y(floattheta1,floattheta2);fp32Angle_Gain[3]={100,0,100};fp32Speed_Gain[3]={200,0,200};floatleg_0_theta_1,leg_0_theta_2,leg_1_theta_1,leg_1_theta_2,leg_2_theta_1,leg_2_theta_2,leg_3_theta_1,leg_3_theta_2;States_estate;States_elast_state;GaitParams_tgait_params;//{stance_height,down_AMP,up_AMP,flight_percent(proportion),step_length,FREQ}GaitParams_tstate_gait_params[]={//{s.h,d.a.,u.a.,f.p.,s.l.,fr.,s.d.}{0.22,0.0,0.0,0.35,0.0,1.0,0.0},//STOP{0.24,0.026f,0.09,0.35,0.08,1.5f,0.0},//TROT{0.30,0.04,0.06,0.35,0.0,2.0f,0.0},//BOUND{0.24,0.026f,0.07,0.35f,0.10,1.5f,0.0},//WALK{0.20,0.05,0.0,0.75,0.0,1.0,0.0},//PRONK{0,0,0,0,0,0,0},//JUMP{0.25,0.05,0.05,0.35,0.0,1.5,0.0},//DANCE{0.25,0.04,0.05,0.2,0.05,1.0,0.0},//HOP{0,0,0,0,0,1.0,0},//TEST{0.20,0.02,0.08,0.35f,0,1.0f,0.0},//ROTATE{0.25,0.07,0.06,0.2,0.0,1.0,0.0},//FLIP{0.25,0.04,0.06,0.35,0.1,2.0,0.06},//TURN_TROT{0,0,0,0,0.13,0,0},//RESET {0,0,0,0,0,0,0},//INIT {0.24f,0.02f,0.10f,0.30f,0.0,1.5f,0}//Stepping};externvoidTrans_Jacobian(floatTheta_1,floatTheta_2,floatcurrent_1,floatcurrent_2);voidCoupledMoveLeg(uint8_tleg_num,floatt,GaitParams_t*params,floatgait_offset,floatleg_direction){SinTrajectory(t,params,gait_offset);CartesianToThetaGamma(params,leg_direction);//坐標(biāo)轉(zhuǎn)換角度Theta_Gamma_To_moto_angle(params,leg_num);//轉(zhuǎn)換為水平與上關(guān)節(jié)角度}voidstatic_CoupledMoveLeg(uint8_tleg_num,floatt,GaitParams_t*params,floatgait_offset,floatleg_direction,floatx,floaty){ params->x=x;params->y=y;CartesianToThetaGamma(params,leg_direction);//坐標(biāo)轉(zhuǎn)換角度Theta_Gamma_To_moto_angle(params,leg_num);//轉(zhuǎn)換為水平與上關(guān)節(jié)角度}voidPositionControlThread(void*pvParameters){ uint32_ttime_add; last_state=state=INIT; gait_params=state_gait_params[state]; Leg_Moto_Data_Init(); Leg_Moto_Data_Update(Angle_Gain,Speed_Gain); for(;;) {// Trans_Jacobian(get_Leg_moto_Measure_Point(1)->total_angle,get_Leg_moto_Measure_Point(0)->total_angle,\// get_Leg_moto_Measure_Point(0)->given_current/1000.0f,get_Leg_moto_Measure_Point(1)->given_current/1000.0f); time_add++; if(time_add%10==0){// printf("--%d--",HAL_GetTick());// Ni_Ming(0xf1,get_Leg_moto_Measure_Point(6)->given_current/1000.0f,get_Leg_moto_Measure_Point(7)->given_current/1000.0f,\// get_Leg_moto_Measure_Point(0)->given_current/1000.0f,get_Leg_moto_Measure_Point(1)->given_current/1000.0f);// Ni_Ming(0xf1,Leg_Move.Moto_Angle_Pid[6].fdb,get_Leg_moto_Measure_Point(7)->round_cnt,\// Leg_Move.Moto_Angle_Pid[7].set,Leg_Move.Moto_Angle_Pid[7].fdb);// Ni_Ming(0xf1,Detect_Judeg(1),Detect_Judeg(2),0,0); } State_Trans();//狀態(tài)轉(zhuǎn)換 if(last_state!=state) { gait_params=state_gait_params[state];//更新步態(tài)參數(shù)// ramp_init(&state_change_ramp,state_change_ramp_value); } last_state=state;// Angle_Gain[0]=P_1;Angle_Gain[2]=D_1;Speed_Gain[0]=P_2;Speed_Gain[2]=D_2; switch(state){ caseINIT:{ NULL; }break; caseSTOP:{ Angle_Gain[0]=350;Angle_Gain[1]=0.0f;Angle_Gain[2]=250;Speed_Gain[0]=450;Speed_Gain[1]=0;Speed_Gain[2]=300; Leg_Moto_Data_Update(Angle_Gain,Speed_Gain); gait(&gait_params,0,0,0,0,1,1,1,1); }break; caseRESTART:{ ExecuteRest(); }break; caseDANCE:{// gait(&gait_params,0.0,0.5,0.0,0.5); }break; caseBOUND:{// gait(&gait_params,0.0,0.5,0.5,0.0); }break; caseTROT:{ Angle_Gain[0]=200;Angle_Gain[1]=0;Angle_Gain[2]=250;Speed_Gain[0]=450;Speed_Gain[1]=0;Speed_Gain[2]=300; Leg_Moto_Data_Update(Angle_Gain,Speed_Gain); gait(&gait_params,0.0,0.5,0.5,0,\ sign_int16(get_remote_control_point()->rc.ch[4]),sign_int16(get_remote_control_point()->rc.ch[4]),\ sign_int16(get_remote_control_point()->rc.ch[4]),sign_int16(get_remote_control_point()->rc.ch[4])); }break; caseTURN_TROT:{// gait(&gait_params,0.0,0.5,0.0,0.5); }break; caseWALK:{ Angle_Gain[0]=260;Angle_Gain[2]=250;Speed_Gain[0]=400;Speed_Gain[2]=130; Leg_Moto_Data_Update(Angle_Gain,Speed_Gain); gait(&gait_params,0.0,0.5,0.25,0.75,\ sign_int16(get_remote_control_point()->rc.ch[4]),sign_int16(get_remote_control_point()->rc.ch[4]),\ sign_int16(get_remote_control_point()->rc.ch[4]),sign_int16(get_remote_control_point()->rc.ch[4])); }break; casePRONK:{// gait(&gait_params,0.0,0.0,0.0,0.0); }break; caseJUMP:{ ExecuteJump(); }break; caseROTATE:{ Angle_Gain[0]=120;Angle_Gain[2]=100;Speed_Gain[0]=220;Speed_Gain[2]=130; Leg_Moto_Data_Update(Angle_Gain,Speed_Gain); if(get_remote_control_point()->rc.ch[0]>0){//右轉(zhuǎn) gait(&gait_params,0.75,0.5,0.0,0.25,\ -sign_int16(get_re