基于DSP的全向運動控制系統(tǒng)軟件設(shè)計

1、基于DSP的全向運動控制系統(tǒng)軟件設(shè)計 基于DSP的全向運動控制系統(tǒng)軟件設(shè)計 摘 要 本文基于DSP C2000系列TMS320LF2407A核心控制芯片，以CCStudio V3.3軟件為開發(fā)平臺，在了解RoboCup中型組足球機器人和其他全向機器人的基礎(chǔ)上，主要完成了全向運動控制系統(tǒng)的軟件設(shè)計。通過對全向運動控制系統(tǒng)的研究，建立了三軸全向運動學(xué)數(shù)學(xué)模型，并對整體平移運動、原地旋轉(zhuǎn)運動、邊平移邊旋轉(zhuǎn)三種運動方式進行分析和數(shù)學(xué)建模；利用上位機，經(jīng)無線模塊發(fā)送運動方式和各種運行參數(shù)，控制運動系統(tǒng)實現(xiàn)各種運動模式；經(jīng)全向運動控制系統(tǒng)軟件的編寫和與相關(guān)硬件聯(lián)機調(diào)試，系統(tǒng)實現(xiàn)了預(yù)訂的全向運動形式。通過試

2、驗驗證和結(jié)果分析，各種運動狀態(tài)的準確性已達到性能的基本要求。關(guān)鍵詞 DSP；全向運動控制； 數(shù)學(xué)建模 ；MATLAB仿真 ；串行通信;軟件設(shè)計Software Design of Omni-directional Motion Control System Based on DSPAbstract: This article is based on The TMS320LF2407A in the series of DSP C2000 as the control core chip and the software of CCStudio V3.3 as the development p

3、latform. It is basis on the understanding of medium-sized group about soccer robot in Robocup and other omnidirectional robot, mainly accomplish the software design of a Omni-directional Motion Control System. Through the research to the motion control system, motion control system was established t

4、o all three axis kinematics mathematic model, build the mathematical modeling and analysis three kind of movement ,which include the whole translation movement, spin around movement and edge translation and rotating.Using the PC, The wireless module to send the movement and a variety of operating pa

5、rameters, control motion system to achieve a variety of motion modes; By the whole motion control system software to write and online calibration with relevant hardware, the system realized the omnidirectional movement we expected. Through the results analysis and experimental verification, the accu

6、racy have already meet requirements of various sports state.Key Words: DSP; omnidirectional motion control; mathematical modeling; MATLAB simulation; serial communication；software design目 錄1 引言11.1 全向運動控制系統(tǒng)發(fā)展現(xiàn)狀11.2 本課題的研究意義及前景21.3 論文組織結(jié)構(gòu)32 全向運動控制系統(tǒng)分析32.1 全向運動控制系統(tǒng)運動學(xué)模型建立32.2不同運動方式的運動特性52.2.1 平移運動52.2

7、.2 原地旋轉(zhuǎn)運動72.3.3邊平移邊旋轉(zhuǎn)運動83 基于DSP的硬件系統(tǒng)簡介83.1 控制芯片選擇83.2 硬件系統(tǒng)結(jié)構(gòu)圖93.3 硬件系統(tǒng)基本模塊104 系統(tǒng)運動控制的MATLAB仿真134.1 電機PID控制134.2 轉(zhuǎn)速檢測144.3 MATLAB仿真155 系統(tǒng)軟件設(shè)計165.1 軟件開發(fā)平臺及仿真器165.2 運動控制軟件設(shè)計185.2.1 主程序185.2.2 三種基本運動狀態(tài)子程序195.2.3 電機控制子程序205.2.4 無線發(fā)送子程序205.3 上位機軟件及通訊協(xié)議216 實驗驗證及結(jié)果分析226.1試驗場地236.2 各項性能測試246.2.1 速度PID測試246.2

8、.2 平移運動測試246.2.3 原地旋轉(zhuǎn)測試256.2.4 平移+旋轉(zhuǎn)運動測試266.3 影響因素分析26結(jié)束語28參考文獻29附錄30致謝42III基于DSP的全向運動控制系統(tǒng)軟件設(shè)計1 引言隨著機器人技術(shù)的日新月異，機器人應(yīng)用領(lǐng)域也已從工業(yè)走向普通生活。機器人技術(shù)已經(jīng)在人們的生活中越來越發(fā)揮著重要的作用。作為機器人中的全向運動機器人，有著其特殊的運動形式，即可以不改變姿勢向任何方向運動，找到最佳位置。同時也可以邊往某個方向運動，邊調(diào)整自身的姿勢達到最佳運動效果，可以在平面內(nèi)完美的往任何方向做三自由度運動。全向運動機器人其高靈活度，勢必會在機器人領(lǐng)域發(fā)揮不可代替的作用。在全向運動機器人中現(xiàn)

9、階段比較常見的是三輪結(jié)構(gòu)和四輪結(jié)構(gòu)（如圖1所示）。通過控制各個輪子間相互協(xié)調(diào)運動達到全方位的運動效果，包括全方位平移、全方位邊平移邊旋轉(zhuǎn)和原地旋轉(zhuǎn)運動形式。要完整文章畢業(yè)論文的+扣扣：（149-908-83-24）去掉中間的橫線圖1 三輪和四輪的結(jié)構(gòu)模型2 全向運動控制系統(tǒng)分析2.1 全向運動控制系統(tǒng)運動學(xué)模型建立在引言中介紹了全向運動控制的幾種結(jié)構(gòu)，本設(shè)計選用三輪結(jié)構(gòu)構(gòu)建全向運動系統(tǒng)34。其結(jié)構(gòu)圖和實物付如圖4所示 。 (a) 結(jié)構(gòu)圖 (b) 實物圖 圖4 全向結(jié)構(gòu)圖和實物圖由圖5中(b)所示，V為機器人系統(tǒng)整體速度，世界坐標X-Y坐標系中X軸與整體速度V的逆時針夾角為，在世界坐標系中將整體

10、速度V分解成沿X軸和Y軸的速度分別為和。2.2不同運動方式的運動特性 全向運動的運動形式大體可分為平移運動、原地旋轉(zhuǎn)運動、邊平移邊旋轉(zhuǎn)運動三種運動形式。三種運動形式有著不同的運動特性，以下將分三小節(jié)對三種運動形式進行單獨分析。2.2.1 平移運動平移是基本的也是比較常用的運動形式。由于是平移不考慮旋轉(zhuǎn)運動，我們將模型進行簡化，其簡化后運動學(xué)模型圖如圖6所示。 (a) 平移模型 (b) 系統(tǒng)整體平移模型圖6 簡化后運動運動學(xué)模型單獨考慮平移運動故將式(3)進行簡化為式(4)： (4)假設(shè)小車整體平移矢量速度為V，與X軸成角。所以我們可以分解為和，其分解公式為式(5)。 (5)由式(4)和式(5)

11、我們可以得到三個驅(qū)動輪的速度，見式(6)。 (6)驅(qū)動輪速度、的速度范圍都是。的值則由電機性能決定。通過式(6)我們可以反向推導(dǎo)出平移速度V的速度范圍。如圖7，其中圓是以驅(qū)動輪最大速度大小為半徑的圓，紅色速度V與X軸的角度為，隨著由0°360°變化V的軌跡為紅色正六邊形。其中最大速度為。圖7 平移最大速度模型3 基于DSP的硬件系統(tǒng)簡介3.1 控制芯片選擇作為運動控制系統(tǒng)的核心控制芯片，根據(jù)以往在實驗室的經(jīng)驗，其核心控制芯片當(dāng)屬DSP C2000系類。C2000系類對于運動控制有著諸多的優(yōu)點，本設(shè)計采用C2000系類TMS320LF2407A的控制板。TMS320LF240

12、7A主要特點如下： 3.2 硬件系統(tǒng)結(jié)構(gòu)圖 通過第2章關(guān)于運動學(xué)模型的建立，為使得系統(tǒng)能夠按照建立的數(shù)學(xué)模型進行運動，除了3.1節(jié)的DSP主控制芯片外還需要其它的硬件的支持，本課題設(shè)計的硬件系統(tǒng)結(jié)構(gòu)圖如圖9所示。其主要的工作方式為： PC上位機選擇相應(yīng)的運動方式，經(jīng)過RS232協(xié)議連接無線模塊，由無線模塊發(fā)送數(shù)據(jù)。DSP下位機的無線模塊接收上位機發(fā)送的數(shù)據(jù)，經(jīng)RS232通信協(xié)議發(fā)送給DSP。DSP接收到數(shù)據(jù)進行相應(yīng)的處理，分析出要進行的運動形式及各種參數(shù)，同時利用I2C協(xié)議采集指南針的數(shù)據(jù)，將這些信息通過第2章的全向運動學(xué)模型分析的各種公式轉(zhuǎn)換成各個電機的速度值。為使得電機能夠穩(wěn)、準、快的執(zhí)行

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

14、制系統(tǒng)使用三軸全向控制，即控制3個電機相互協(xié)調(diào)運動來實現(xiàn)全向輪動。電機的控制對整個系統(tǒng)是至關(guān)重要的。本章主要分析系統(tǒng)的PID控制和MATLAB仿真。4.1 電機PID控制電機是運動系統(tǒng)的執(zhí)行機構(gòu)，不管DSP計算的多么正確，但是電機執(zhí)行達不到要求，同樣系統(tǒng)是無法實現(xiàn)全向運動的。為使得電機能夠穩(wěn)、準、快的執(zhí)行，而PID是根據(jù)偏差的比例、積分、微分的線性組合進行反饋控制，是迄今為止工業(yè)中應(yīng)用最為廣泛的一種控制方法。所以本設(shè)計采用PID分別對三個電機進行閉環(huán)控制。4.2 轉(zhuǎn)速檢測 本設(shè)計選用增量式PID即4.1節(jié)中所闡述的PID。對于PID控制反饋必不可少，在本設(shè)計使用的是Faulhaber帶雙路編碼

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

16、果比較曲線如圖15所示。綠色曲線為期望值，藍色曲線為PID控制跟蹤曲線，紅色曲線為誤差曲線，實際速度可以通過編碼器反饋?？梢钥闯鯬ID控制方式的響應(yīng)速度、跟蹤精度可以達到系統(tǒng)要求。要完整文章畢業(yè)論文的+扣扣：（149-908-83-24）去掉中間的橫線5 系統(tǒng)軟件設(shè)計根據(jù)前幾章模型建立分析和硬件系統(tǒng)構(gòu)建，本章主要介紹全向運動控制系統(tǒng)程序編寫。前兩節(jié)介紹了下位機全向運動系統(tǒng)使用的程序編寫平臺軟件和整個系統(tǒng)流程圖及各部分子程序的流程圖。最后一節(jié)介紹上位機部分，編寫上位機軟件部分和上位機下位機使用的通訊協(xié)議。5.1 軟件開發(fā)平臺及仿真器本設(shè)計的核心控制芯片為TMS320LF2407A，其軟件開發(fā)平臺

17、使用官方工具CCS v3.3，仿真器選用XDS510-USB2.0。安裝CCS V3.3后生成兩個部分，即CCStudio V3.3 和Setup CCStudio v3.3 。其中利用Setup CCStudio v3.3對開發(fā)工具參考硬件進行相應(yīng)配置，利用CCStudio V3.3建立工程編寫軟件,其開發(fā)周期示意圖為圖18所示12。圖19即為CCStudio V3.3開發(fā)環(huán)境。5.2 運動控制軟件設(shè)計由于本課題是基于DSP全向運動控制系統(tǒng)軟件設(shè)計，側(cè)重控制全向運動，故本設(shè)計將檢測部分做了精簡，只需要采集指南針和編碼脈沖。其全向運動方式經(jīng)過上位機無線傳輸指令，然后DSP接收指令進行處理最后控

18、制電機協(xié)調(diào)運動，根據(jù)指南針和編碼器進行閉環(huán)控制，以達到最佳效果。6 實驗驗證及結(jié)果分析 為驗證本設(shè)計的各種全向運動是否準確，本章將對各種全向運動形式進行試驗驗證。設(shè)計試驗場地，記錄測試試驗數(shù)據(jù)，對結(jié)果進行分析。6.1試驗場地 由于本設(shè)計使用的驅(qū)動輪為尼龍材質(zhì)。驅(qū)動輪與地面接觸部分硬度較大，如果在普通地板上驅(qū)動輪將得不到足夠大的摩擦力，有嚴重的“打滑”現(xiàn)象。為提供足夠摩擦力結(jié)合實驗室條件，選擇飛思卡爾賽道使用的KT板，做成120CM*100CM的試驗場地，并在場地上標上邊距為20cm的方格，同時為驗證轉(zhuǎn)角正確性標上了相隔45°的虛線，結(jié)構(gòu)圖見圖26。6.2 各項性能測試 需要試驗驗證測

19、試的有：PID驗證、平移驗證、原地旋轉(zhuǎn)驗證、邊平移邊旋轉(zhuǎn)運動。設(shè)計制作試驗裝置，分別對各種性能測試。6.3 影響因素分析 影響系統(tǒng)性能的因素大部分來源于外部，大致分為以下幾種情況：（1） 電池電量不足，導(dǎo)致系統(tǒng)電機啟動或大的變化時電流不足。導(dǎo)致系統(tǒng)不能很好的按照設(shè)定的形式運動。(2） 外部磁場干擾，數(shù)字指南針能夠很靈敏的感應(yīng)磁場，所以外部稍強點的磁場就能干擾系統(tǒng)，一方面是金屬鐵，其很容易磁化，導(dǎo)致變成弱磁鐵。在室內(nèi)鋼筋結(jié)構(gòu)的環(huán)境下各個位置受到的磁干擾不同，經(jīng)測試最大可影響角度達30°角，嚴重影響了系統(tǒng)性能。另一方面是電機，由于電機帶有磁性較強磁鐵，產(chǎn)生磁場的磁場也對指南針有一定干擾。

20、去除干擾的方法只能經(jīng)過硬磁補償來減少很少部分干擾，大多數(shù)干擾無法去除。(3） 驅(qū)動輪干擾，本設(shè)計使用的是瑞典全向輪，輪子外側(cè)有兩排相互交錯的側(cè)輪，使得驅(qū)動輪滾動過程中，造成上下震動。同時由于驅(qū)動輪滾動會造成電機力矩發(fā)生變化，L=12或14.5cm，及內(nèi)外兩個側(cè)滑輪著地時會造成力矩2.5cm的變化，會使得系統(tǒng)轉(zhuǎn)動發(fā)生誤差。要完整文章畢業(yè)論文的+扣扣：（149-908-83-24）去掉中間的橫線結(jié)束語全向運動控系統(tǒng)在全向機器人上的應(yīng)用可實現(xiàn)機器人向任意方向做直線運動而不需要事先作旋轉(zhuǎn)運動，并且在以直線運動到達目標點的過程中同時可以做自身旋轉(zhuǎn)運動，以調(diào)整機器人的姿態(tài)，從而達到終態(tài)所需的姿態(tài)角。其特殊

21、的移動方式使得全向機器人越來越得到重視和應(yīng)用，其未來的實用領(lǐng)域和前景將更加寬廣。本設(shè)計完成的主要工作為以下幾點：(1) 對目前的幾種運動結(jié)構(gòu)進行了研究分析，設(shè)計了互成120度的三軸運動結(jié)構(gòu)。(2) 對三軸運動控制結(jié)構(gòu)進行了數(shù)學(xué)建模，進行了運動學(xué)分析。對全向運動的運動的幾種運動形式進行了詳細的分析計算。(3) 運用TMS320LF2407A芯片為控制核心，使用了其模塊UART（SCI）、IIC（模擬）、PWM、I/O、CAP、外部擴展RAM。同時圍繞核心的外部器件進行程序的編寫調(diào)試，已達到全向控制的目的。(4) 使用了VC6.0的MSComm控件編寫上位機軟件，設(shè)計上位機界面和功能，編寫通信協(xié)議

22、。使得系統(tǒng)能夠經(jīng)過選擇相關(guān)運動形式，修改上位機的相關(guān)參數(shù)來實現(xiàn)運動系統(tǒng)的全向運動。同時能夠在上位機顯示系統(tǒng)現(xiàn)在運動的狀態(tài)和三個電機速度值。并實現(xiàn)了通過游戲手柄來遙控系統(tǒng)的各種運動狀態(tài)。(5) 對多電機實現(xiàn)了數(shù)字增量式PID控制，三個電機各自PID控制且互不影響，并通過MATLAB進行了仿真。使得計算出的速度能夠很好的執(zhí)行，提高了系統(tǒng)的準確性。(6) 實現(xiàn)了全向運動控制系統(tǒng)的所有運動形式，且經(jīng)過試驗測試驗證了其準確性。參考文獻1 徐旭,李實,葉榛,孫增圻.A Survey: RoboCup and the Research Proceedings of the 3rd World Congres

23、s on Intelligent Control and AutomationM,20082 趙冬斌,易建強,鄧旭玥.全方位移動機器人結(jié)構(gòu)和運動分析J.機器人.2003(25) 3 海丹.全向移動平臺的設(shè)計與控制D.長沙:國防科技大學(xué). 20094 龔建偉.Visual C+/Turbo C串口通信編程實踐. 北京.電子工業(yè)出版社，20045 RaulRojas:Omnidirectional Control,FreieUniversityBerlin,Technical Report B-10-03, FU.Bedin.June 20036 劉和平.DSP原理及電機控制應(yīng)用基于TMS320L

24、F240X系列M.北京:北京航空航天大學(xué)出版社,2008.7 王茂飛,程昱.TMS320C2000DSP技術(shù)與應(yīng)用開發(fā)M.北京:北京清華大學(xué)出版社,2007.8 Texas Instruments.TMS320LF/LC240xA DSP Controllers Reference Guide-System and Peripherals SPRU357BM.December,2001.9 陶永華，尹怡欣，葛蘆生.新型PID控制及其應(yīng)用M.北京.機械工業(yè)出版社.199810 薛定宇.控制系統(tǒng)計算機輔助設(shè)計-MATLAB語言與應(yīng)用（第2版）M.北京:清華大學(xué)出版社,2006. 11 范海艇.基于

25、Nios的中型足球機器人底層控制系統(tǒng)的研究與實現(xiàn)D.上海,上海大學(xué). 12 姜艷波.數(shù)字信號處理器-DSP應(yīng)用100例M，北京：化學(xué)工業(yè)出版社，200713 海丹.全向移動平臺的設(shè)計與控制D.長沙:國防科技大學(xué)，200614 張宏林.精通Visual C+串口通信技術(shù)與工程實踐(第3版) .北京. 人民郵電出版社，200815 龔建偉.Visual C+/Turbo C串口通信編程實踐（第二版）M,北京：電子工業(yè)出版社，2007.9附錄 基于DSP的全新運動控制系統(tǒng)軟件清單如下:其中自己建的有GB161.H（存儲液晶字模數(shù)據(jù)），24C02.C（操作指南針），design.C（運動控制策略），I

26、LI9163.C（液晶顯示），main.c（主程序）。由于程序比較大，在此只附錄上main.c和design.c兩個文件。Main.c ：/*/* File Name : main.c */* Autor : Li Hai-qing */* Date : 04/01/2011 */* Description : This file provides main function, provide initialization */* 串口發(fā)送與中斷接收程序,波特率設(shè)置為9600 */* IO口占用：PE1-PE6（PWM） PA3-PA6和PB0-PB7和PE7液晶屏使用 */* PF0=CAP

27、5,PF1=cap6,PA3=cap1三電機測速。PE2=PWM8 PE4=PWM10，PE6=PWM12三電機 */* PC0-1 PF2-3 PF4-5電機正反轉(zhuǎn) */*/#include "global.c" #include "math.h"void SystemInit();void Timer1Init();void ILI9163_init(); void KickDog(); void PWM_Init();void IOinit();void Timer4Init();void Timer2Init();/*/* 主程序 */*/mai

28、n() SystemInit(); /系統(tǒng)初始化 IOinit(); /IO口初始化 SCI_Init(); /SCI初始化 PWM_Init(); /PWM初始化 ILI9163_init(); /液晶初始化 Timer1Init(); /*定時器1初始化*/ Timer4Init(); /*定時器4初始化*/ Timer2Init(); /*定時器2初始化*/ asm(" CLRC INTM "); display(0xff07,0xffe0); LCD_PutString(40,03,"李海清",0x001f,0x07e0); / 液晶界面顯示信息

29、 LCD_PutString(16,23,"運動控制系統(tǒng)",0x001f,0x07e0); / LCD_PutString(0,143,"指南針:",0x001f,0x07e0); / while(1) SciRed(); /判斷并讀取SCI數(shù)據(jù) speedPV(); /速度函數(shù) /*/* 系統(tǒng)初始化 */*/void SystemInit() asm(" SETCINTM "); /* 關(guān)閉總中斷 */ asm(" CLRC SXM "); /* 禁止符號位擴展 */ asm(" CLRC CNF &q

30、uot;); /* B0塊映射為 on-chip DARAM*/ asm(" CLRC OVM "); /* 累加器結(jié)果正常溢出*/ SCSR1=0x834E; /* 系統(tǒng)時鐘CLKOUT=20*2=40M */ /* 打開ADC,EVA,EVB,CAN和SCI的時鐘*/ WDCR=0x006F; /* 禁止看門狗,看門狗時鐘64分頻 */ KickDog(); /* 初始化看門狗 */ IFR=0xFFFF; /* 清除中斷標志 */ IMR=0x000B; /* 打開中斷1,2*/ /*/* IO口初始化 */*/void IOinit() MCRC=MCRC &

31、; 0x0354; /PE0-PE7，PF0-PF5有效 0000 MCRA=MCRA & 0x0008; /PA0-PA7，PB0-PB7有效0208 MCRB=MCRB & 0xFF00; PADATDIR=0xF708; /PA口初始化值為0 PBDATDIR=0xFF00; /PB口初始化值為0 PEDATDIR=0xFF7F; /PE口初始化值為0 PFDATDIR=0xFC33; /PF0-5為1 PF6輸入 PCDATDIR=0xFFFF; /pc輸出 pc7-pc0 0101 0101/*/* PWM初始化 */*/void PWM_Init() MCRC=MC

32、RC | 0x0054; /PE1-PE6為PWM口 7E EVBIFRA=0xFFFF; / 清除中斷標志 ACTRB=0x0666; /PWM12,10,8低有效 ,PWM 11,9,7高有效 /DBTCONA=0x0530; /使能死區(qū)定時器1,分頻40M/16=2.5M,死區(qū)時間5*0.4us=2us T3PR=2500; /定時器3周期值,定時0.4us*2500=1ms /*2500 CMPR4=0; /比較值 CMPR5=0; /比較值 CMPR6=0; /比較值 COMCONB=0xA600; /比較控制寄存器 T3CNT=0; /定時器3計數(shù)器清零 EVBIMRA=0x008

33、0; /定時器3周期中斷使能 T3CON=0x144E; /增模式, TPS系數(shù)40M/16=2.5M,T3使能, void KickDog() /*踢除看門狗 */ WDKEY=0x5555; WDKEY=0xAAAA;design.c ：/*/* File Name : design.c */* Autor : Li Hai-Qing */* Date : 03/10/2011 */* Description : This file provides Motion control strategy */*/#include "GLOBAL.C"#include &quo

34、t;math.h"#define speed1 CMPR4 /宏定義speed1電機1速度#define speed2 CMPR5 /宏定義speed1電機2速度#define speed3 CMPR6 /宏定義speed1電機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 /宏定義PIunsigned int flog=0,flogF=0,flogH=0,flogX=0,flogPID=

35、0; /各種運動狀態(tài)標志位unsigned int flogPV=0,pvlcd=0,flogPV1=0,flogPV2=0,flogPV3=0; unsigned int 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,speed

36、3_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;unsigned int tt55=0,tt66=0,tt11=0; /電機編碼器值tt55 tt66 tt11分別電機1-3的離散值unsigned int scitt99=0,0,0,0,0,0,0,0,0; unsigned int PVlb=0,CY_26_read=0,CY_26_Angle=0;unsigned int SCIHspeed=0,SCIHangle=0,SCIHag_SN=0; unsigned int SCIXspeed=0

37、,SCIXangle=0; unsigned int flogHStop=0,flogXSN=0;float flogangle=0.0,angle11=0.0,angle22=0.0,angle33=0.0,angle=0.0;char SCI_RXDATA114=0,0,0,0,0,0,0,0,0,0,0,0,0,0;/*SCI初始化*/void SCI_Init() MCRA=MCRA | 0x0003;/IOPA0, IOPA1為串口SCICCR=0x0007; /空閑多處理器模式，8位數(shù)據(jù)位,1位停止位，無奇偶校驗位SCICTL1=0x0003; /接收，發(fā)送，內(nèi)部時鐘使能，SLEE

38、P=1SCICTL2=0x0002; /接收中斷使能SCIPRI=0x0000; /高中斷優(yōu)先級SCIHBAUD=0x0002; /40M,波特率設(shè)置為9600SCILBAUD=0x0008; SCICTL1=0x0023; /串口初始化完成 /* 定時器1初始化*/void Timer1Init() T1PR=2500; / 定時器1初值,定時0.4us*2500=1ms T1CNT=0; GPTCONA=0x0000; EVAIMRA=EVAIMRA|0x0080; / 定時器1周期中斷使能 EVAIFRA=EVAIFRA|0xFFFF; / 清除中斷標志T1CON=0x144E; /增模

39、式, TPS系數(shù)40M/16=2.5M,T1使能 /* 定時器1-2初始化*/void Timer2Init() T2PR=2500; T2CNT=0; WSGR=0x0000; CAPCONA=0x2240; CAPFIFOA=0x0100; EVAIMRC=EVBIMRC|0x0001; EVAIFRC=EVBIFRC|0xffff; T2CON=0x1440; /1440/*定時器4初始化 輸入捕捉*/void Timer4Init() T4PR=2500; T4CNT=0; WSGR=0x0000; CAPCONB=0x3014; CAPFIFOB=0x1500; EVBIMRC=EV

40、BIMRC|0x0006; EVBIFRC=EVBIFRC|0xffff; T4CON=0x1440; /1440/*SCI發(fā)送*/void SCI_Send( unsigned data) /發(fā)送0x80-0x87, 共8個字節(jié)數(shù)據(jù) SCITXBUF=data; while(SCICTL2 & 0x0080)=0); /*SCI接收分解運動數(shù)據(jù)*/void SCIF()/執(zhí)行速度分解程序，分別控制三個電機的速度unsigned int i=1;for(i=1;i<14;i+) RXDINTi=SCI_RXDATA1i-48; /接收轉(zhuǎn)換 if(flogF=0|flogX=1|f

41、logH=1) LCD_clear(0,63,128,64,0xff07,0xffe0); flogX=0; flogH=0; LCD_PutString(16,63 ,"分解運動",0x001f,0x07e0); LCD_PutString(0 ,83 ,"電機1:",0x001f,0x07e0); LCD_PutString(0 ,103,"電機2:",0x001f,0x07e0); LCD_PutString(0 ,123,"電機3:",0x001f,0x07e0); SCISP1=RXDINT1*1000+

42、RXDINT2*100+RXDINT3*10+RXDINT4;SCISP2=RXDINT5*1000+RXDINT6*100+RXDINT7*10+RXDINT8; SCISP3=RXDINT9*1000+RXDINT10*100+RXDINT11*10+RXDINT12;LCD_Num(48,83 ,SCISP1,0x001f,0x07e0);LCD_Num(48,103,SCISP2,0x001f,0x07e0);LCD_Num(48,123,SCISP3,0x001f,0x07e0);/電機1PC0-1 電機2 pf2-3 電機3 pf4-5PCDATDIR=0xFFFF;PFDATDI

43、R=PFDATDIR|0xFCFC; if(RXDINT1=1) PCDATDIR=PCDATDIR&0xFFFE; else PCDATDIR=PCDATDIR&0xFFFD;if(RXDINT5=1) PFDATDIR=PFDATDIR&0xFCFB; else PFDATDIR=PFDATDIR&0xFCF7;if(RXDINT9=1) PFDATDIR=PFDATDIR&0xFCEF; else PFDATDIR=PFDATDIR&0xFCDF;sum11=SCISP1%1000;sum22=SCISP2%1000;sum33=SCISP

44、3%1000;if(sum11=0) PCDATDIR=PCDATDIR|0xFFFF; flogPV1=0; else flogPV1=1;if(sum22=0) PFDATDIR=PFDATDIR|0xFC0C; flogPV2=0; else flogPV2=1;if(sum33=0) PFDATDIR=PFDATDIR|0xFC30; flogPV3=0; else flogPV3=1;flogF=1; /分解運動標志位，控制顯示部分清屏，顯示相應(yīng)運動方式和數(shù)據(jù)/*SCI接收合成平移運動數(shù)據(jù)*/void SCIH() /執(zhí)行速度合成平移運動，控制整體速度和角度兩個值 float CYa

45、nglePI=0.0,CYangleSD=0.0;int CYspeed=0;int flogAnglePI=0,flogaa=0; int HSDangle=0; int HSD_SCIangle=0,absangle=0; HSDangle=flogangle*180/PI+180;if(HSDangle>=360) HSDangle=HSDangle-360; HSD_SCIangle=HSDangle-CY_26_Angle; /設(shè)定角度與實際角度的差值absangle=abs(HSD_SCIangle); /設(shè)定與實際差距的絕對值 if(RXDINT10=1) if(absan

46、gle>180) if(HSD_SCIangle>-358&&HSD_SCIangle<-180) CYspeed=15+(360-absangle)/5; else if(HSD_SCIangle<358&&HSD_SCIangle>180) CYspeed=-15-(360-absangle)/5; else CYspeed=0; if(absangle=180) Cyspeed=15; else ; /防止抖動 if(absangle<180) if(HSD_SCIangle>1&&HSD_SCIa

47、ngle<180) CYspeed=15+absangle/5; else if(HSD_SCIangle<-1&&HSD_SCIangle>-180) CYspeed=-15-absangle/5; else CYspeed=0; else CYspeed=0; CYangleSD=PI/2-flogangle; if(flogangle<=PI) flogAnglePI=(int)(flogangle*180/PI +180); else flogAnglePI=(int)(flogangle*180/PI-180); flogaa=CY_26_Angle-flogAnglePI; CYanglePI=PI-CY_26_Angle*PI/180; sum11_0=(int)SCIHspeed*sin(PI/6-CYangleP