基于DSP的全向運(yùn)動控制系統(tǒng)軟件設(shè)計(jì)說明

1、基于DSP的全向運(yùn)動控制系統(tǒng)軟件設(shè)計(jì)摘要基于DSP C2000系列TMS320LF2407A核心控制芯片和CCStudio V3.3軟件為開發(fā)平臺，在了解RoboCup中型足球機(jī)器人等全向機(jī)器人的基礎(chǔ)上，進(jìn)行全向運(yùn)動控制系統(tǒng)的軟件設(shè)計(jì)主要完成。 .通過對全向運(yùn)動控制系統(tǒng)的研究，建立了三軸全向運(yùn)動學(xué)的數(shù)學(xué)模型，對整體平移運(yùn)動、原地旋轉(zhuǎn)運(yùn)動、自轉(zhuǎn)運(yùn)動三種運(yùn)動模式進(jìn)行了分析和數(shù)學(xué)建模。在進(jìn)行翻譯時；使用上位機(jī)，通過無線模塊致運(yùn)動模式和各種運(yùn)行參數(shù)，控制運(yùn)動系統(tǒng)實(shí)現(xiàn)各種運(yùn)動模式；通過全向運(yùn)動控制系統(tǒng)軟件的編寫和相關(guān)硬件的在線調(diào)試，系統(tǒng)實(shí)現(xiàn)了預(yù)定的全向運(yùn)動形式。通過實(shí)驗(yàn)驗(yàn)證和結(jié)果分析，各種運(yùn)動狀態(tài)的精度

2、都達(dá)到了性能的基本要求。關(guān)鍵詞 DSP；全向運(yùn)動控制；數(shù)學(xué)建模； MATLAB仿真；串行通信；軟件設(shè)計(jì)目錄 TOC o 1-3 h z u HYPERLINK l _Toc292193127 1簡介 PAGEREF _Toc292193127 h 1 HYPERLINK l _Toc292193128 1.1全向運(yùn)動控制系統(tǒng)發(fā)展現(xiàn)狀 PAGEREF _Toc292193128 h 1 HYPERLINK l _Toc292193129 1.2本課題的研究意義與展望 PAGEREF _Toc292193129 h 2 HYPERLINK l _Toc292193130 1.3論文組織結(jié)構(gòu) PAG

3、EREF _Toc292193130 h 3 HYPERLINK l _Toc292193131 2全向運(yùn)動控制系統(tǒng)分析 PAGEREF _Toc292193131 h 3 HYPERLINK l _Toc292193132 2.1全向運(yùn)動控制系統(tǒng)運(yùn)動學(xué)模型的建立 PAGEREF _Toc292193132 h 3 HYPERLINK l _Toc292193133 2.2不同運(yùn)動方式的運(yùn)動特點(diǎn) PAGEREF _Toc292193133 h 5 HYPERLINK l _Toc292193134 2.2.1平移運(yùn)動 PAGEREF _Toc292193134 h 5 HYPERLINK l

4、_Toc292193135 2.2.2原位旋轉(zhuǎn)運(yùn)動 PAGEREF _Toc292193135 h 7 HYPERLINK l _Toc292193136 2.3.3平移時旋轉(zhuǎn) PAGEREF _Toc292193136 h 8 HYPERLINK l _Toc292193137 3 基于DSP的硬件系統(tǒng)介紹 PAGEREF _Toc292193137 h 8 HYPERLINK l _Toc292193138 3.1控制片選 PAGEREF _Toc292193138 h 8 HYPERLINK l _Toc292193139 3.2硬件系統(tǒng)結(jié)構(gòu)圖 PAGEREF _Toc292193139

5、 h 9 HYPERLINK l _Toc292193140 3.3硬件系統(tǒng)基本模塊 PAGEREF _Toc292193140 h 10 HYPERLINK l _Toc292193141 4 MATLAB系統(tǒng)運(yùn)動控制仿真 PAGEREF _Toc292193141 h 13 HYPERLINK l _Toc292193142 4.1電機(jī)PID控制 PAGEREF _Toc292193142 h 13 HYPERLINK l _Toc292193143 4.2速度檢測 PAGEREF _Toc292193143 h 14 HYPERLINK l _Toc292193144 4.3 MATLA

6、B仿真 PAGEREF _Toc292193144 h 15 HYPERLINK l _Toc292193145 5系統(tǒng)軟件設(shè)計(jì) PAGEREF _Toc292193145 h 16 HYPERLINK l _Toc292193146 5.1軟件開發(fā)平臺和模擬器 PAGEREF _Toc292193146 h 16 HYPERLINK l _Toc292193147 5.2運(yùn)動控制軟件設(shè)計(jì) PAGEREF _Toc292193147 h 18 HYPERLINK l _Toc292193148 5.2.1主程序 PAGEREF _Toc292193148 h 18 HYPERLINK l _T

7、oc292193149 5.2.2三個基本運(yùn)動狀態(tài)子程序 PAGEREF _Toc292193149 h 19 HYPERLINK l _Toc292193150 5.2.3電機(jī)控制子程序 PAGEREF _Toc292193150 h 20 HYPERLINK l _Toc292193151 5.2.4無線致子程序 PAGEREF _Toc292193151 h 20 HYPERLINK l _Toc292193152 5.3 上位機(jī)軟件及通訊協(xié)議 PAGEREF _Toc292193152 h 21 HYPERLINK l _Toc292193153 6實(shí)驗(yàn)驗(yàn)證與結(jié)果分析 PAGEREF

8、_Toc292193153 h 22 HYPERLINK l _Toc292193154 6.1測試場地 PAGEREF _Toc292193154 h 23 HYPERLINK l _Toc292193155 6.2各種性能測試 PAGEREF _Toc292193155 h 24 HYPERLINK l _Toc292193156 6.2.1速度PID測試 PAGEREF _Toc292193156 h 24 HYPERLINK l _Toc292193157 6.2.2平移運(yùn)動測試 PAGEREF _Toc292193157 h 24 HYPERLINK l _Toc292193158

9、6.2.3原位旋轉(zhuǎn)試驗(yàn) PAGEREF _Toc292193158 h 25 HYPERLINK l _Toc292193159 6.2.4平移+旋轉(zhuǎn)運(yùn)動測試 PAGEREF _Toc292193159 h 26 HYPERLINK l _Toc292193160 6.3影響因素分析 PAGEREF _Toc292193160 h 26 HYPERLINK l _Toc292193161 結(jié)論 PAGEREF _Toc292193161 h 28 HYPERLINK l _Toc292193162 參考文獻(xiàn) PAGEREF _Toc292193162 h 29 HYPERLINK l _Toc

10、292193163 附錄 PAGEREF _Toc292193163 h 30 HYPERLINK l _Toc292193164 至 PAGEREF _Toc292193164 h 421 簡介隨著機(jī)器人技術(shù)的飛速發(fā)展，機(jī)器人應(yīng)用領(lǐng)域也從工業(yè)走向了日常生活。機(jī)器人技術(shù)在人們的生活中發(fā)揮著越來越重要的作用。作為機(jī)器人中的全向運(yùn)動機(jī)器人，它有其特殊的運(yùn)動形式，即可以在不改變姿態(tài)的情況下向任意方向運(yùn)動，以找到最佳位置。同時，它還可以向某個方向移動并調(diào)整姿態(tài)，以達(dá)到最佳的運(yùn)動效果，并且可以在平面上完美地進(jìn)行任意方向的三自由度運(yùn)動。全向運(yùn)動機(jī)器人的高度靈活性必將在機(jī)器人領(lǐng)域發(fā)揮不可替代的作用。在全向運(yùn)

11、動機(jī)器人中，現(xiàn)階段以三輪結(jié)構(gòu)和四輪結(jié)構(gòu)較為常見（如圖1所示）。通過控制各個車輪的協(xié)調(diào)運(yùn)動，可以實(shí)現(xiàn)全方位的運(yùn)動效果，包括全向平移、全向平移和旋轉(zhuǎn)、原地旋轉(zhuǎn)。完成畢業(yè)論文+按鈕：（149-908-83-24）去掉中間的橫線圖1 三、四輪結(jié)構(gòu)模型2 全向運(yùn)動控制系統(tǒng)分析2.1 全向運(yùn)動控制系統(tǒng)運(yùn)動學(xué)模型的建立在引言中，介紹了全向運(yùn)動控制的幾種結(jié)構(gòu)。在本設(shè)計(jì)中，三輪結(jié)構(gòu)用于構(gòu)建全向運(yùn)動系統(tǒng)34 。其結(jié)構(gòu)圖和實(shí)物支付如圖4所示。(a) 結(jié)構(gòu)圖 (b) 實(shí)物圖圖4 全方位結(jié)構(gòu)圖和物理圖如圖5(b)所示，V為機(jī)器人系統(tǒng)的整體速度，在世界坐標(biāo)XY坐標(biāo)系中X軸與整體速度V的逆時針夾角為。在世界坐標(biāo)系中，整體速

12、度 V 被分解為沿 X 軸和 Y 軸的速度分別為和。2.2 不同運(yùn)動方式的運(yùn)動特點(diǎn)全向運(yùn)動的運(yùn)動形式大致可以分為三種運(yùn)動形式：平移運(yùn)動、原地旋轉(zhuǎn)運(yùn)動和邊平移邊旋轉(zhuǎn)運(yùn)動。三種運(yùn)動形式具有不同的運(yùn)動特性，下面將分三個小節(jié)分別對三種運(yùn)動形式進(jìn)行分析。2.2.1 平移運(yùn)動平移是一種基本且常用的運(yùn)動形式。由于是平移，不考慮旋轉(zhuǎn)運(yùn)動，我們對模型進(jìn)行了簡化，簡化后的運(yùn)動學(xué)模型圖如圖6所示。(a) 翻譯模型 (b) 系統(tǒng)整體翻譯模型圖 6 運(yùn)動學(xué)的簡化運(yùn)動學(xué)模型僅考慮平移運(yùn)動，將公式（3）簡化為公式（4）：(4)假設(shè)汽車的整體平移矢量速度為 V，與 X 軸形成夾角 。因此我們可以將其分解為sum ，其分解公式

13、為式（5）。(5)由式(4)和式(5)可以得到三個驅(qū)動輪的轉(zhuǎn)速，見式(6)。(6)驅(qū)動輪的轉(zhuǎn)速，和轉(zhuǎn)速范圍都是。該值由電機(jī)性能決定。由式(6)可以逆推導(dǎo)出平移速度V的速度范圍。如圖7所示，圓是以驅(qū)動輪最大速度為半徑的圓，與紅色速度V的夾角X軸為。V的軌跡從0變?yōu)?60是一個紅色的正六邊形。其中最大速度為。圖 7 平移最大速度模型3 基于DSP的硬件系統(tǒng)介紹3.1 控制片選作為運(yùn)動控制系統(tǒng)的核心控制芯片，根據(jù)實(shí)驗(yàn)室以往的經(jīng)驗(yàn)，其核心控制芯片屬于DSP C2000系列。 C2000 系列在運(yùn)動控制方面具有許多優(yōu)勢。本設(shè)計(jì)采用C2000系列TMS320LF2407A的控制板。 TMS320LF240

14、7A的主要特點(diǎn)如下：3.2 硬件系統(tǒng)結(jié)構(gòu)圖通過第 2 章運(yùn)動學(xué)模型的建立，為了使系統(tǒng)按照建立的數(shù)學(xué)模型運(yùn)行，除了第 3.1 節(jié)的 DSP 主控芯片外，還需要其他硬件支持。本項(xiàng)目設(shè)計(jì)的硬件系統(tǒng)結(jié)構(gòu)圖如圖所示。如圖 9 所示。其主要工作模式如下：PC主機(jī)選擇相應(yīng)的運(yùn)動模式，通過RS232協(xié)議連接無線模塊，無線模塊致數(shù)據(jù)。 DSP下位機(jī)的無線模塊接收上位機(jī)致的數(shù)據(jù)，通過RS232通訊協(xié)議致給DSP。 DSP接收數(shù)據(jù)并進(jìn)行相應(yīng)的處理，分析運(yùn)動形式和要執(zhí)行的各種參數(shù)，并使用I2C協(xié)議收集羅盤數(shù)據(jù)，并將這些信息轉(zhuǎn)換成第2章全向運(yùn)動學(xué)模型分析的各種公式。每個電機(jī)的速度值。為使電機(jī)運(yùn)行穩(wěn)定、準(zhǔn)確、快速，設(shè)計(jì)采

15、用經(jīng)典的PID控制，增量編碼器采集的數(shù)值反饋。在此過程中，液晶模塊實(shí)時顯示羅盤的運(yùn)動方式和參數(shù)及數(shù)值，同時通過無線模塊將編碼器的數(shù)值傳送給PC主機(jī)。 DSP下位機(jī)會根據(jù)程序設(shè)置執(zhí)行上位機(jī)設(shè)置的運(yùn)動模式，只有在上位機(jī)重新致新的運(yùn)動模式時才會改變。3.3 硬件系統(tǒng)的基本模塊根據(jù)圖9所示的硬件系統(tǒng)結(jié)構(gòu)圖，本節(jié)將分析幾個重要模塊。分為電源模塊、無線通訊模塊、指南針模塊、液晶顯示模塊、DSP下位機(jī)模塊、電機(jī)及驅(qū)動模塊、全向輪。 DSP下位機(jī)在3.1節(jié)已經(jīng)介紹過，這里不再贅述。完成畢業(yè)論文+按鈕：（149-908-83-24）去掉中間的橫線4 系統(tǒng)運(yùn)動控制的MATLAB仿真本設(shè)計(jì)的全向運(yùn)動控制系統(tǒng)采用三軸

16、全向控制，即控制三個電機(jī)的協(xié)調(diào)運(yùn)動，實(shí)現(xiàn)車輪的全向運(yùn)動。電機(jī)的控制對整個系統(tǒng)至關(guān)重要。本章主要分析了系統(tǒng)的PID控制和MATLAB仿真。4.1 電機(jī) PID 控制電機(jī)是運(yùn)動系統(tǒng)的執(zhí)行器，無論DSP計(jì)算多么正確，但電機(jī)執(zhí)行達(dá)不到要求，同一系統(tǒng)無法實(shí)現(xiàn)全向運(yùn)動。為了使電機(jī)能夠穩(wěn)定、準(zhǔn)確、快速地執(zhí)行，PID是基于比例、積分和微分偏差的線性組合的反饋控制。是目前業(yè)界應(yīng)用最廣泛的控制方式。因此本設(shè)計(jì)采用PID分別對三臺電機(jī)進(jìn)行閉環(huán)控制。4.2 速度檢測本設(shè)計(jì)選擇增量 PID，即 4.1 節(jié)中描述的 PID。 PID 控制反饋是必不可少的。在本設(shè)計(jì)中，采用雙編碼器的Faulhaber減速電機(jī)，即增量式光電

17、編碼器作為反饋環(huán)節(jié)。其工作原理如圖14所示。光源照射的信號通過碼盤和檢測光柵，經(jīng)光電檢測裝置檢測，檢測到的正弦值經(jīng)轉(zhuǎn)換電路轉(zhuǎn)換成矩形波，最后輸出A相、B相、Z相。 A相和B相相差90度，通過軟件可以很好的獲得此時電機(jī)的轉(zhuǎn)速和方向。 AB相的獲取是電機(jī)閉環(huán)控制的反饋量，關(guān)系到電機(jī)PID控制能否滿足穩(wěn)定性、精度和速度的要求。本設(shè)計(jì)采用輸入捕捉CAP功能對脈沖進(jìn)行采集，對采集量進(jìn)行相應(yīng)處理，得到PID控制的反饋。4.3 MATLAB 仿真在三軸控制系統(tǒng)中， PID是應(yīng)用最廣泛的控制算法。對于電機(jī)模型，這里設(shè)置電機(jī)模型，對該電機(jī)模型進(jìn)行MATLAB仿真，觀察PID控制算法的控制效果1011 ?？刂平Y(jié)果

18、對比曲線如圖15所示，綠色曲線為期望值，藍(lán)色曲線為PID控制跟蹤曲線，紅色曲線為誤差曲線。實(shí)際速度可以通過編碼器反饋?？梢姡?PID控制方法的響應(yīng)速度和跟蹤精度均能滿足系統(tǒng)要求。完成畢業(yè)論文+按鈕：（149-908-83-24）去掉中間的橫線5 系統(tǒng)軟件設(shè)計(jì)根據(jù)前幾章的模型建立分析和硬件系統(tǒng)搭建，本章主要介紹全向運(yùn)動控制系統(tǒng)的編程。前兩節(jié)介紹了下位機(jī)全向運(yùn)動系統(tǒng)所使用的編程平臺軟件、整個系統(tǒng)的流程圖和各部分子程序的流程圖。最后一節(jié)介紹了上位機(jī)部分，編寫了上位機(jī)的軟件部分以及上位機(jī)下位機(jī)使用的通信協(xié)議。5.1 軟件開發(fā)平臺和模擬器本設(shè)計(jì)的核心控制芯片為TMS320LF2407A，其軟件開發(fā)平臺使

19、用官方工具CCSv3.3 ，仿真器使用XDS510-USB2.0。安裝 CCS V3.3 后，會生成兩個部分，分別是 CCStudio V3.3 和Setup CCStudio v3.3 。其中， Setup CCStudio v3.3用于配置開發(fā)工具的參考硬件，CCStudio V3.3用于建立項(xiàng)目編寫軟件。開發(fā)周期示意圖如圖 18 12所示。圖 19 是 CCStudio V3.3 開發(fā)環(huán)境。5.2 運(yùn)動控制軟件設(shè)計(jì)由于本課題是基于DSP全向運(yùn)動控制系統(tǒng)的軟件設(shè)計(jì)，側(cè)重于全向運(yùn)動的控制，因此本設(shè)計(jì)中簡化了檢測部分，只需要采集羅盤和編碼脈沖。其全向運(yùn)動方式通過上位機(jī)無線傳輸指令，然后DSP接

20、收指令進(jìn)行處理，最終控制電機(jī)的協(xié)調(diào)運(yùn)動，并根據(jù)羅盤和編碼器進(jìn)行閉環(huán)控制，以達(dá)到最佳效果。6 實(shí)驗(yàn)驗(yàn)證與結(jié)果分析為了驗(yàn)證本設(shè)計(jì)中各種全向運(yùn)動的準(zhǔn)確性，本章將對各種全向運(yùn)動形式進(jìn)行測試和驗(yàn)證。設(shè)計(jì)測試場地，記錄測試數(shù)據(jù)，分析結(jié)果。6.1 測試地點(diǎn)因?yàn)檫@種設(shè)計(jì)中使用的驅(qū)動輪是由尼龍制成的。驅(qū)動輪與地面接觸部分的硬度比較大。如果驅(qū)動輪在普通地板上，摩擦力不夠，就會出現(xiàn)嚴(yán)重的“打滑”現(xiàn)象。為了提供足夠的摩擦力結(jié)合實(shí)驗(yàn)室條件，選用飛思卡爾賽道所用的KT板做一個120CM*100CM的測試場地，在場地上標(biāo)出一個留有20cm邊距的正方形，同時驗(yàn)證拐角的正確性。相隔45的虛線結(jié)構(gòu)圖如圖26所示。6.2 各種性能

21、測試需要驗(yàn)證的測試有：PID驗(yàn)證、平移驗(yàn)證、原位旋轉(zhuǎn)驗(yàn)證、邊平移邊旋轉(zhuǎn)。設(shè)計(jì)和制造測試裝置，分別測試各種性能。6.3影響因素分析影響系統(tǒng)性能的因素大部分來自外部，大致可以分為以下幾種情況：(1)電池電量不足，導(dǎo)致系統(tǒng)電機(jī)啟動或變化較大時電流不足。結(jié)果，系統(tǒng)不能很好地按照設(shè)定的形式移動。(2)外部磁場干擾，數(shù)字羅盤對磁場的感應(yīng)非常靈敏，所以稍強(qiáng)的外部磁場會對系統(tǒng)產(chǎn)生干擾。一方面是金屬鐵，易被磁化，產(chǎn)生弱磁。在機(jī)房鋼結(jié)構(gòu)環(huán)境下，各個位置受到的磁干擾不同，經(jīng)測試最大影響角可達(dá)30，嚴(yán)重影響系統(tǒng)性能。另一方面是電機(jī)。由于電動機(jī)具有強(qiáng)磁鐵，因此產(chǎn)生磁場的磁場也會干擾指南針。去除干擾的方法只能通過硬磁補(bǔ)償

22、減少一小部分干擾，大部分干擾無法去除。(3)主動輪干涉，本設(shè)計(jì)采用瑞典全向輪，車輪外側(cè)有兩排交錯的側(cè)輪，使主動輪在滾動過程中上下振動。同時，由于驅(qū)動輪的滾動，電機(jī)扭矩會發(fā)生變化。當(dāng) L=12 或 14.5cm 時，當(dāng)兩個外滑輪著地時，扭矩會發(fā)生 2.5cm 的變化，這會導(dǎo)致系統(tǒng)轉(zhuǎn)動出現(xiàn)誤差。完成畢業(yè)論文+按鈕：（149-908-83-24）去掉中間的橫線結(jié)束語全向機(jī)器人上的全向運(yùn)動控制系統(tǒng)可以實(shí)現(xiàn)機(jī)器人在任意方向進(jìn)行直線運(yùn)動，而無需事先進(jìn)行旋轉(zhuǎn)運(yùn)動。姿態(tài)，從而達(dá)到最終狀態(tài)所需的姿態(tài)角。其特殊的移動方式使得全向機(jī)器人越來越受到重視和應(yīng)用，其未來的實(shí)用領(lǐng)域和前景將更加廣闊。本次設(shè)計(jì)完成的主要工作如

23、下：(1) 研究分析了目前幾種運(yùn)動結(jié)構(gòu)，設(shè)計(jì)了互成120度的三軸運(yùn)動結(jié)構(gòu)。(2)對三軸運(yùn)動控制結(jié)構(gòu)進(jìn)行數(shù)學(xué)建模，并進(jìn)行運(yùn)動學(xué)分析。詳細(xì)分析計(jì)算了全向運(yùn)動的幾種運(yùn)動形式。(3) 以TMS320LF2407A芯片為控制核心，采用其模塊UART（SCI）、IIC（模擬）、PWM、I/O、CAP、外接擴(kuò)展RAM。同時圍繞核心外接設(shè)備編寫和調(diào)試程序，達(dá)到了全方位控制的目的。(4)利用VC6.0的MSComm控件編寫上位機(jī)軟件，設(shè)計(jì)上位機(jī)的接口和功能，編寫通信協(xié)議。這使得系統(tǒng)通過選擇相關(guān)運(yùn)動形式，修改上位機(jī)相關(guān)參數(shù)，實(shí)現(xiàn)運(yùn)動系統(tǒng)的全方位運(yùn)動。同時，上位機(jī)可顯示系統(tǒng)當(dāng)前狀態(tài)及三個電機(jī)轉(zhuǎn)速值。并通過游戲手柄實(shí)

24、現(xiàn)遙控系統(tǒng)的各種運(yùn)動狀態(tài)。(5) 實(shí)現(xiàn)了多臺電機(jī)的數(shù)字增量PID控制，三臺電機(jī)由各自的PID控制，互不影響，通過MATLAB進(jìn)行仿真。計(jì)算出的速度可以很好地執(zhí)行，提高了系統(tǒng)的精度。(6) 實(shí)現(xiàn)了全向運(yùn)動控制系統(tǒng)的所有運(yùn)動形式，并通過實(shí)驗(yàn)測試驗(yàn)證了其準(zhǔn)確性。參考1 徐旭，石，葉真，曾祺。綜述：RoboCup與第三屆世界智能控制與自動化大會研究論文集M ，200 82 董斌，易建強(qiáng)，鄧旭月。全向移動機(jī)器人結(jié)構(gòu)與運(yùn)動分析J機(jī)器人。 2003(25)3 海丹全方位移動平臺的設(shè)計(jì)與控制D.：國防科技大學(xué)。 20094龔建偉. Visual C+/Turbo C 串行通信編程實(shí)踐。電子工業(yè), 20045

25、Raul Rojas：全方位控制，柏林自由大學(xué)，技術(shù)報(bào)告 B-10-03，F(xiàn)U。貝丁。 2003 年 6 月。6 和平 DSP原理與電機(jī)控制應(yīng)用基于TMS320LF240X系列M.：航空航天大學(xué)，2008.7 王茂飛，程宇 TMS320C2000DSP技術(shù)與應(yīng)用開發(fā)M.：清華大學(xué)，2007.8 Texas Instruments.TMS320LF/LC240 xA DSP Controllers Reference Guide-System and Peripherals SPRU357BM.December,2001.9 永華，一欣，盧生。新型PID控制及其應(yīng)用M機(jī)械工業(yè)。 199810 薛

26、定宇控制系統(tǒng)的計(jì)算機(jī)輔助設(shè)計(jì)MATLAB語言與應(yīng)用（第2版）M.：清華大學(xué)，2006.11 海丁基于Nios II的中型足球機(jī)器人底層控制系統(tǒng)的研究與實(shí)現(xiàn)D.大學(xué)。12 嚴(yán)波. 100個數(shù)字信號處理器-DSP應(yīng)用實(shí)例M，：化工，200713 海丹全方位移動平臺的設(shè)計(jì)與控制D.：國防科技大學(xué)，200614 洪林精通Visual C+串行通信技術(shù)與工程實(shí)踐（第3版）.人民郵電，200815 龔建偉. Visual C+/Turbo C串口通信編程實(shí)戰(zhàn)（第二版）M，：電子工業(yè)，2007.9附錄基于DSP的新型運(yùn)動控制系統(tǒng)軟件清單如下：其中有GB161.H（存儲液晶字體數(shù)據(jù)）、24C02.C（操作羅盤

27、）、design.C（運(yùn)動控制策略）、ILI9163.C（液晶顯示）、main.c（主程序）。因?yàn)槌绦虮容^大，所以本附錄只包含了main.c和design.c兩個文件。主要.c ：/* * */* 文件名：main.c */* 作者：李海清* /* 日期： 04/01/2011 */* 說明：該文件提供main函數(shù)，提供初始化 */* 串口致和中斷接收程序，波特率設(shè)置為9600 */* IO口占用：PE1-PE6（PWM）PA3-PA6和PB0-PB7和PE7液晶屏使用*/* PF0=CAP5, PF1=cap6, PA3=cap1 三電機(jī)測速。 PE2=PWM8 PE4=PWM10, PE6

28、=PWM12 三電機(jī)*/* PC0-1 PF2-3 PF4-5 電機(jī)正反轉(zhuǎn) */* * */#include global.c#include math.h無效系統(tǒng)初始化（）；無效 Timer1Init();無效 ILI9163_init();無效踢狗（）；無效 PWM_Init();無效 IOinit();無效 Timer4Init();無效 Timer2Init();/* * */* 主程序 */* * */主要的（）系統(tǒng)初始化（）； /系統(tǒng)初始化IOinit(); /IO口初始化SCI_Init(); /SCI初始化PWM_Init(); /PWM初始化ILI9163_init(); /

29、液晶初始化Timer1Init(); /*定時器1初始化*/Timer4Init(); /*定時器4初始化*/Timer2Init(); /*定時器2初始化*/asm(CLRC INTM);顯示（0 xff07,0 xffe0）； LCD_PutString(40,03,海清,0 x001f,0 x07e0); / LCD界面顯示信息LCD_PutString(16,23,運(yùn)動控制系統(tǒng),0 x001f,0 x07e0); /LCD_PutString(0,143,指南針:,0 x001f,0 x07e0); /而（1）科學(xué)紅（）； /判斷和讀取SCI數(shù)據(jù)速度PV（）； /速度函數(shù)/* */*

30、系統(tǒng)初始化*/* */無效系統(tǒng)初始化（）asm(SETC INTM); /* 禁用總中斷 */asm(CLRC SXM); /* 禁用符號擴(kuò)展 */asm(CLRC CNF); /* B0 塊映射到片上 DARAM*/asm(CLRC OVM); /* 累加器結(jié)果正常溢出 */SCSR1=0 x834E； /* 系統(tǒng)時鐘 CLKOUT=20*2=40M */ /* 開啟ADC、EVA、EVB、CAN、SCI的時鐘*/WDCR=0 x006F； /* 禁用看門狗，看門狗時鐘除以64*/踢狗（）； /* 初始化看門狗*/IFR=0 xFFFF； /* 清除中斷標(biāo)志*/IMR=0 x000B； /*

31、 打開中斷 1,2*/* */* IO口初始化 */* */無效 IOinit()MCRC=MCRC & 0 x0354; /PE0-PE7，PF0-PF5有效0000MCRA=MCRA & 0 x0008; /PA0-PA7,PB0-PB7有效0208MCRB=MCRB & 0 xFF00;PADATDIR=0 xF708； /PA口初始化值為0PBDATDIR=0 xFF00； /PB端口初始值為0佩達(dá)迪爾=0 xFF7F； /PE口初始值為0PFDATDIR=0 xFC33； /PF0-5為1個PF6輸入PCDATDIR=0 xFFFF； /pc輸出pc7-pc0 0101 0101/*

32、 */* PWM 初始化 */* */無效 PWM_Init()MCRC=MCRC | 0 x0054; /PE1-PE6為PWM端口7EEVBIFRA=0 xFFFF； / 清除中斷標(biāo)志ACTRB=0 x0666； /PWM12,10,8低有效，PWM 11,9,7高有效/DBCTONA=0 x0530; /啟用死區(qū)定時器1，分頻40M/16=2.5M，死區(qū)時間5*0.4us=2usT3PR=2500； /定時器3周期值，定時0.4us*2500=1ms /*2500CMPR4=0； /比較值CMPR5=0； /比較值CMPR6=0； /比較值COMCONB=0 xA600； /比較控制寄存

33、器T3CNT=0； /定時器3計(jì)數(shù)器清零EVBIMRA=0 x0080； /定時器3周期中斷使能T3CON=0 x144E； /增加模式，TPS系數(shù)40M/16=2.5M，T3開啟，void KickDog() /*踢看門狗*/WDKEY=0 x5555；WDKEY=0 xAAAA；設(shè)計(jì).c：/* * */* 文件名：design.c */* 作者：李海清 */* 日期：2011 年 3 月 10 日 */*描述：該文件提供運(yùn)動控制策略*/* * */#include GLOBAL.C#include math.h#define speed1 CMPR4 /宏定義speed1電機(jī)1速度#def

34、ine speed2 CMPR5 /宏定義speed1電機(jī)2速度#define speed3 CMPR6 /宏定義speed1電機(jī)2速度#define PVtime 3 /掃描時間，每個輪子檢測PVtime ms#define Kp 0.8 /宏定義Kp#define Ki 0.2 /宏定義 Ki#define Kd 0.2 /宏定義Kd#define PI 3.14159 /宏定義PI無符號整數(shù) flog=0,flogF=0,flogH=0,flogX=0,flogPID=0; /各種運(yùn)動狀態(tài)標(biāo)志無符號整數(shù) flogPV=0,pvlcd=0,flogPV1=0,flogPV2=0,flogP

35、V3=0;無符號整數(shù) SCI_RXDATA,RXDINT14=0,0,0,0,0,0,0,0,0,0,0,0,0,0;int sum11=0 ,sum22=0 ,sum33=0;int sum11_0=0,sum22_0=0,sum33_0=0;int SCIn=0, i=0, SCISP1, SCISP2, SCISP3;int speed1_1=0,speed2_1=0,speed3_1=0,speed1_0=0,speed2_0=0,speed3_0=0;int tt5=0,tt6=0,tt1=0,t0=0,tt5_33=0,0,0,tt6_33=0,0,0,tt1_33=0 ,0,0;

36、無符號整數(shù) tt55=0,tt66=0,tt11=0; /電機(jī)編碼器值tt55 tt66 tt11分別為電機(jī)1-3的離散值無符號整數(shù) scitt99=0,0,0,0,0,0,0,0,0;無符號整數(shù) PVlb=0,CY_26_read=0,CY_26_Angle=0;無符號整數(shù) SCIHspeed=0,SCIHangle=0,SCIHag_SN=0;無符號整數(shù) SCIXspeed=0,SCIXangle=0;無符號整數(shù) flogHStop=0,flogXSN=0;浮動 flogangle=0.0,angle11=0.0,angle22=0.0,angle33=0.0,angle=0.0;字符 S

37、CI_RXDATA114=0,0,0,0,0,0,0,0,0,0,0,0,0,0;/*SCI初始化*/無效 SCI_Init()MCRA=MCRA | 0 x0003; /IOPA0,IOPA1為串口SCICCR=0 x0007； /空閑多處理器模式，8個數(shù)據(jù)位，1個停止位，無奇偶校驗(yàn)位SCICTL1=0 x0003； /接收，致，外部時鐘使能，SLEEP=1SCICTL2=0 x0002； /接收中斷使能SCIPRI=0 x0000； /高中斷優(yōu)先級SCIHBAUD=0 x0002； /40M，波特率設(shè)置為9600SCILBAUD=0 x0008；SCICTL1=0 x0023； /串口初始

38、化完成/* 定時器 1 初始化 * * /無效 Timer1Init()T1PR=2500； /定時器1初始值，計(jì)時0.4us*2500=1msT1CNT=0；GPTCNA=0 x0000；EVAIMRA=EVAIMRA|0 x0080; / 定時器 1 周期中斷使能EVAIFRA=EVAIFRA|0 xFFFF; / 清除中斷標(biāo)志T1CON=0 x144E； /增加模式，TPS系數(shù)40M/16=2.5M，T1開啟/*定時器1-2初始化* * */無效 Timer2Init()T2PR=2500；T2CNT=0；WSGR=0 x0000；卡普納=0 x2240；CAFIFOA=0 x0100；

39、EVAIMRC=EVBIMRC|0 x0001；EVAIFRC=EVBIFRC|0 xffff；T2CON=0 x1440； /1440/*定時器4初始化輸入捕捉*/無效 Timer4Init()T4PR=2500；T4CNT=0；WSGR=0 x0000；CAPCONB=0 x3014；CAPFIFOB=0 x1500；EVBIMRC=EVBIMRC|0 x0006；EVBIFRC=EVBIFRC|0 xffff；T4CON=0 x1440； /1440/*SCI致*/void SCI_Send(unsigned data) /致0 x80-0 x87，共8字節(jié)數(shù)據(jù)SCITXBUF=數(shù)據(jù)；而

40、(SCICTL2 & 0 x0080)=0);/*SCI接收分解運(yùn)動數(shù)據(jù)*/void SCIF() /執(zhí)行速度分解程序，分別控制三個電機(jī)的速度無符號整數(shù) i=1;for(i=1;i=360）HSDangle=HSDangle-360；HSD_SCIangle=HSDangle-CY_26_Angle； /設(shè)置角度與實(shí)際角度的差值absangle=abs(HSD_SCIangle); /設(shè)置實(shí)際間隙的絕對值如果（RXDINT10=1）如果（絕對角 180）if(HSD_SCIangle-358&HSD_SCIangle-180) CYspeed=15+(360-absangle)/5;否則 if

41、(HSD_SCIangle180) CYspeed=-15-(360-absangle)/5;否則 CYspeed=0;if(absangle=180) C y速度=15;別的 ; /防止如果（絕對角1&HSD_SCIangle180) CYspeed=15+absangle/5;否則 if(HSD_SCIangle-180) CYspeed=-15-absangle/5;否則 CYspeed=0;否則 CYspeed=0;CYangleSD=PI/2-flogangle；if(flogangle=0）PCDATDIR=PCDATDIR&0 xFFFE；否則 PCDATDIR=PCDATDIR

42、&0 xFFFD;如果（sum22_0=0）PFDATDIR=PFDATDIR&0 xFCFB；否則 PFDATDIR=PFDATDIR&0 xFCF7;如果（sum33_0=0）PFDATDIR=PFDATDIR&0 xFCEF；否則 PFDATDIR=PFDATDIR&0 xFCDF;sum11=abs(sum11_0);sum22=abs(sum22_0);sum33=abs(sum33_0);if(flogHStop=1) sum11=0;sum22=0;sum33=0; else ;if(sum11=0) PCDATDIR=PCDATDIR|0 xFFFF; flogPV1=0； 否

43、則 flogPV1=1;if(sum22=0) PFDATDIR=PFDATDIR|0 xFC0C; flogPV2=0； 否則 flogPV2=1;if(sum33=0) PFDATDIR=PFDATDIR|0 xFC30; flogPV3=0； 否則 flogPV3=1;如果（flog=1）LCD_Putnum(48,83,SCIHspeed,0 x001f,0 x07e0);LCD_Num(48,103,SCIHag_SN,0 x001f,0 x07e0);flogH=1； /合成平移運(yùn)動標(biāo)志，控制顯示部分清屏，顯示對應(yīng)的運(yùn)動模式和數(shù)據(jù)/*SCI接收旋轉(zhuǎn)運(yùn)動數(shù)據(jù)*/void SCIX()

44、 /執(zhí)行旋轉(zhuǎn)運(yùn)動，控制旋轉(zhuǎn)整數(shù) SDangle=0；int SD_SCIangle=0,Xspeed=0,absangle=0;SD角度=SCIX角度+180；如果（SDangle=360）SDangle=SDangle-360；SD_SCIangle=SDangle-CY_26_Angle；absangle=abs(SD_SCIangle);如果（絕對角 180）if(SD_SCIangle-358&SD_SCIangle-180) Xspeed=15+( 360- absangle )/2;否則 if(SD_SCIangle180) Xspeed=-15-( 360- absangle )

45、/2;否則 Xspeed=0;如果（絕對角度=180）Xspeed=15；別的 ; /防止如果（絕對角1&SD_SCIangle180) Xspeed=15+absangle/2;否則 if(SD_SCIangle-180) Xspeed=-15-absangle/2;否則 Xspeed=0;sum11_0=X 速度；sum22_0=X 速度；sum33_0=X 速度；PCDATDIR=0 xFFFF;PFDATDIR=PFDATDIR|0 xFCFC;如果（sum11_0=0）PCDATDIR=PCDATDIR&0 xFFFE；否則 PCDATDIR=PCDATDIR&0 xFFFD;如果（

46、sum22_0=0）PFDATDIR=PFDATDIR&0 xFCFB；否則 PFDATDIR=PFDATDIR&0 xFCF7;如果（sum33_0=0）PFDATDIR=PFDATDIR&0 xFCEF；否則 PFDATDIR=PFDATDIR&0 xFCDF;sum11=abs(sum11_0);sum22=abs(sum22_0);sum33=abs(sum33_0);if(sum11=0) PCDATDIR=PCDATDIR|0 xFFFF; flogPV1=0； 否則 flogPV1=1;if(sum22=0) PFDATDIR=PFDATDIR|0 xFC0C; flogPV2=

47、0； 否則 flogPV2=1;if(sum33=0) PFDATDIR=PFDATDIR|0 xFC30; flogPV3=0； 否則 flogPV3=1;如果（flog=1）LCD_Putnum(48,83,SCIHspeed,0 x001f,0 x07e0);LCD_Num(48,103,SCIHag_SN,0 x001f,0 x07e0);flogX=1;/循環(huán)運(yùn)動標(biāo)志位，控制顯示部分清屏，顯示對應(yīng)的運(yùn)動模式和數(shù)據(jù)/*速度PID* * */int ee1=0,ee2=0,ee3=0,ee1_2=0,ee1_1=0,ee2_2=0,ee2_1=0,ee3_2=0,ee3_1=0;無效 P

48、IDPV()ee1_2 = ee1_1；ee1_1 = ee1;ee2_2 = ee2_1；ee2_1 = ee2;ee3_2 = ee3_1；ee3_1 = ee3;ee1 = sum11-tt55；ee2 = sum22-tt66；ee3 = sum33-tt11；if (flogPID = 1) speed1_1 = 1; speed2_1 = 0; speed3_1 = 0; flogPID = 0; /speed1_1 = speed1_1 + Kp * (ee1-ee1_1) + Ki * ee1 + Kd * (ee1-2 * ee1_1 + ee1_2); / Kp Ki Kd

49、speed2_1 = speed2_1 + Kp * (ee2-ee2_1) + Ki * ee2 + Kd * (ee2-2 * ee2_1 + ee2_2)；speed3_1 = speed3_1 + Kp * (ee3-ee3_1) + Ki * ee3 + Kd * (ee3-2 * ee3_1 + ee3_2)；如果（速度1_1 0）速度1_1 = 0；如果（速度2_1 0）速度2_1 = 0；如果（速度3_1 500）速度1_1=500；如果（速度2_1500）速度2_1=500；如果（速度3_1500）速度3_1=500；如果（flogPV1=1）速度1=速度1_1*5；如果（f

50、logPV2=1）速度2=速度2_1*5；如果（flogPV3=1）速度3=速度3_1*5；/* SCI閱讀，判斷動作形式* * */無效的 SciRed()無符號 i2=0;如果（SCI_RXDATA10=42&flog=1）if(SCI_RXDATA11=35) /合成翻譯對于(i2=2;i213;i2+)RXDINTi2=SCI_RXDATA1i2-48； /處理接收到的SCI如果（flogH=0|flogF=1|flogX=1） /清除運(yùn)動顯示區(qū)域，開始(x, y Tuyu?x1, y1) 部分屏幕。開始 0,83，結(jié)束 128,143LCD_clear(0,63,128,64,0 x

51、ff07,0 xffe0);flogF=0； flogX=0；LCD_PutString(8,63,合成運(yùn)動；平移,0 x001f,0 x07e0);LCD_PutString(0,83,速度:,0 x001f,0 x07e0);LCD_PutString(0,103,角度：,0 x001f,0 x07e0); 別的 ;SCIHspeed=RXDINT2*100+RXDINT3*10+RXDINT4； /翻譯速度SCIHangle=RXDINT6*100+RXDINT7*10+RXDINT8；/平移角度，RXDINT5為順時針和逆時針，1為順時針，2為逆時針SCIHag_SN=RXDINT5*

52、1000+RXDINT6*100+RXDINT7*10+RXDINT8；flogHStop=RXDINT9；if(RXDINT5=1) flogangle=SCIHangle*2*PI/360;/變換角度值如180變換3.14否則 角度=360-SCIHangle; flogangle=角度*2*PI/360； /變換角度值如180變換3.14SCIH(); /合成運(yùn)動子程序執(zhí)行函數(shù)flog=0； /致接收標(biāo)志位1接收，0不接收。在這里清零。else if(SCI_RXDATA11=64) / 旋轉(zhuǎn)對于(i2=2;i213;i2+)RXDINTi2=SCI_RXDATA1i2-48;/處理接收到的SCI如果（flogX=0|flogF=1|flogH=1）LCD_clear(0,63,128,64,0 xff07,0 xffe0); LCD_PutString(8,63,旋轉(zhuǎn)運(yùn)動,0 x001f,0 x07e0);LCD_PutString(0,83,速度:,0 x001f,0 x07e0);LCD_PutString(0,103,角度：,0 x001f,0 x07e0);flogF=0； flogH=0；flogXSN=RXDINT2； /順時針為1，逆時針為2SCIXspeed=RXDINT6*100+RXDINT7*10+RX