【正文】 /* 清除中斷標(biāo)志 */ IMR=0x000B。 /* 禁止看門狗 ,看門狗時鐘 64分頻 */ KickDog()。 /* 累加器結(jié)果正常溢出 */ SCSR1=0x834E。 /* 禁止符號位擴(kuò)展 */ asm( CLRC CNF )。 //速度函數(shù) } } /************************************************/ /* 系統(tǒng)初始化 */ /************************************************/ void SystemInit() { asm( SETC INTM )。 // while(1) { SciRed()。 // 液晶界面顯示信息 LCD_PutString(16,23,運動控制系統(tǒng) ,0x001f,0x07e0)。 display(0xff07,0xffe0)。 /*定時器 4初始化 */ Timer2Init()。 //液晶初始化 Timer1Init()。 //SCI初始化 PWM_Init()。 //系統(tǒng)初始化 IOinit()。 void Timer2Init()。 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 31 void IOinit()。 void KickDog()。 void Timer1Init()。 ： /******************************************************************************/ /* File Name : */ /* Autor : Li Haiqing */ /* Date : 04/01/2021 */ /* Description : This file provides main function, provide initialization */ /* 串口發(fā)送與中斷接收程序 ,波特率設(shè)置為 9600 */ /* IO口占用：： PE1PE6（ PWM） PA3PA6和 PB0PB7和 PE7液晶屏使用 */ /* PF0=CAP5,PF1=cap6,PA3=cap1三電機(jī)測速。 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 29 參考文獻(xiàn) [1] 徐旭 ,李實 ,葉榛 ,孫增圻 .A Survey: RoboCup and the Research Proceedings of the 3rd World Congress on Intelligent Control and Automation[M],2021 [2] 趙冬斌 ,易建強(qiáng) ,鄧旭玥 .全方位移動機(jī)器人結(jié)構(gòu)和運動分析 [J].機(jī)器人 .2021(25) [3] 海丹 .全向移動平臺的設(shè)計與控制 [D].長沙 :國防科技大學(xué) . 2021 [4] 龔建偉 .Visual C++/Turbo C串口通信編程實踐 . 北京 .電子工業(yè)出版社 ， 2021 [5] RaulRojas:Omnidirectional Control,FreieUniversityBerlin,Technical Report B1003, 2021． [6] 劉和平 .DSP 原理及電機(jī)控制應(yīng)用 — 基于 TMS320LF240X 系列 [M].北京 :北京航空航天大學(xué)出版社 ,2021. [7] 王茂飛 ,程昱 .TMS320C2021DSP技術(shù)與應(yīng)用開發(fā) [M].北京 :北京清華大學(xué)出 版 社 ,2021. [8] Texas Peripherals SPRU357B[M].December,2021. [9] 陶永華，尹怡欣，葛蘆生 .新型 PID控制及其應(yīng)用 [M].北京 .機(jī)械工業(yè)出版社 .1998 [10] 薛定宇 .控制系統(tǒng)計算機(jī)輔助設(shè)計 MATLAB 語言與應(yīng)用（第 2 版） [M].北京 :清華大學(xué)出版社 ,2021. [11] 范海艇 .基于 NiosⅡ的中型足球機(jī)器人底層控制系 統(tǒng)的研究與實現(xiàn) [D].上海 ,上海大學(xué) . [12] 姜艷波 .數(shù)字信號處理器 DSP應(yīng)用 100例 [M]，北京：化學(xué)工業(yè)出版社， 2021 [13] 海丹 .全向移動平臺的設(shè)計與控制 [D].長沙 :國防科技大學(xué) ， 2021 [14] 張宏林 .精通 Visual C++串口通信技術(shù)與工程實踐 (第 3版 ) .北京 . 人民郵電出版社， 2021 [15] 龔建偉 .Visual C++/Turbo C串口通信編程實踐（第二版） [M],北京：電子工業(yè)出版社， 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 30 附錄 基于 DSP 的全新運動控 制系統(tǒng)軟件清單如下 : 其中自己建的有 （存儲液晶字模數(shù)據(jù)） ， （操作指南針）， （運動控制策略）， （液晶顯示）， （主程序） 。 使得 計算出的速度能夠很好的執(zhí)行，提高了系統(tǒng)的準(zhǔn)確性。并實現(xiàn)了通過游戲手柄來遙控系統(tǒng)的各種運動狀態(tài)。使得系統(tǒng)能夠經(jīng)過 選擇相關(guān)運動形式， 修改上位機(jī) 的相關(guān)參數(shù)來實現(xiàn)運動系統(tǒng)的全向運動。同時圍繞核心的外部器件進(jìn)行程序的編寫調(diào)試，已達(dá)到全向控制的目的。對全向運動的運動的幾種運動形式進(jìn)行了詳細(xì)的分析計算 。本設(shè)計完成的主要工作為以下幾點： (1) 對目前的幾種運動結(jié)構(gòu)進(jìn)行了研究分析， 設(shè)計了互成 120 度的三軸運動結(jié)構(gòu)。 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 28 結(jié)束語 全向 運動控系統(tǒng)在全向機(jī)器人上的應(yīng)用 可實現(xiàn)機(jī)器人向任意方向做直線運動而不需要事先作旋轉(zhuǎn)運動，并且在以直線運動到達(dá)目標(biāo)點的過程中同時可以做自身旋轉(zhuǎn)運動，以調(diào)整機(jī)器人的姿態(tài)，從而達(dá)到終態(tài)所需的姿態(tài)角。 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 27 (3） 驅(qū)動輪干 擾，本設(shè)計使用的是瑞典全向輪，輪子外側(cè)有兩排相互交錯的側(cè)輪，使得驅(qū)動輪滾動過程中，造成上下震動。 另一方面是電機(jī)，由于電機(jī)帶有磁性較強(qiáng)磁鐵，產(chǎn)生磁場的磁場也對指南針有一定干擾。 在室內(nèi)鋼筋結(jié)構(gòu)的環(huán)境下各個位置受到的磁干擾不同，經(jīng)測試最大可影響角度達(dá) 30176。導(dǎo)致系統(tǒng)不能很好的按照設(shè)定的形式運動。 圖 31 系統(tǒng)平移 +旋轉(zhuǎn)曲線 經(jīng)顯示測試，其最終坐標(biāo)在（ 82 78），誤差約等于在 3cm。 平移 +旋轉(zhuǎn)運動 測試 全向運動特與其他運動形式最大的區(qū)別在于它 能 二維平面內(nèi)不但能夠不做任何準(zhǔn)備的情況下 往任意方向平移，但最高級運動形式為在全向平移的基礎(chǔ)上能夠調(diào)整自身姿態(tài)也就是平移過程中自身能夠旋轉(zhuǎn)。轉(zhuǎn)角示意圖 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 26 通過指南針顯示能夠很清晰的看到，最終轉(zhuǎn)到的角度已經(jīng)達(dá)到了設(shè)定角度，誤差在? 1176。 圖 30 0176。到 45176。同時可以參考試驗場地標(biāo)出互成 45176。其快速性和精確性為系統(tǒng)準(zhǔn)確的方位提供可能。 圖 28 留痕裝置 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 25 下圖 29 為 系統(tǒng)沿 x 軸負(fù)方向平移的曲線圖，起始位置為（ 110 20），結(jié)束位置為（ 10 ），即走過 1m的距離誤差△ e=，最大 。 電機(jī)P I D 速度響應(yīng)曲線01020304050607080901001101201301401501601701801902002100 20 40 60 80 100 120 140 160 180 200 220 240 260 280 300時間m s編碼器值（3ms計數(shù)值）PWM200PIDPWM200 圖 27 電機(jī) 實際 PID效果 曲線 平移運動 測試 平移運動時時全向運動的最主要部分， 其是否準(zhǔn)確穩(wěn)定關(guān)乎全向運動系統(tǒng)整體的成敗。下面紅色線為開環(huán)曲線，上面深藍(lán)色曲線為 PID的效果曲線。在系統(tǒng)中每隔 3 ms 記錄一次 3ms 內(nèi)記錄數(shù)。設(shè)計制作試驗裝置，分別對各種性能測試。的虛線 ，結(jié)構(gòu)圖見圖 26。驅(qū)動輪與地面接觸部分硬度較大，如果在普通地板上驅(qū)動輪 將 得不到足夠大的摩擦力，有嚴(yán)重的“打滑”現(xiàn)象。設(shè)計試驗場地，記錄測試試驗數(shù)據(jù)，對結(jié)果進(jìn)行分析。 *112021582105*： 即 分別控制三電機(jī)， 電機(jī) 1 速度正轉(zhuǎn) 120，電機(jī) 2 正轉(zhuǎn) 58，電機(jī) 3反轉(zhuǎn) 105。小車也將采集到的速度值經(jīng)過無線模塊傳送給上位機(jī)。 表 5 簡單 通信協(xié)議 命令分類 協(xié)議格式 備注 速度分解 *xaaaybbbzccc* “ *”為開始和結(jié)束標(biāo)志，“ aaa”、“ bbb”、“ ccc”為電機(jī) 速度，“ xyz”為電機(jī)正反轉(zhuǎn)標(biāo)志 1 為正轉(zhuǎn)， 2 為反轉(zhuǎn) 速度平移 *aaaxbbb0y00* “ *”為開始結(jié)束標(biāo)志，“ ” 代表速度合成平移標(biāo)志， “ aaa”整體速度，“ x”為順時針或逆時針平移（ 1為順時針 2 為逆時針），“ y”為機(jī)器人方向是否跟隨移動方向（ 1 為擺向移動的方向， 0 為頭不隨移動而改變） 原地旋轉(zhuǎn) *xaaabbb0000* “ *”為開始結(jié)束標(biāo)志，“ ”標(biāo)志原地旋轉(zhuǎn)標(biāo)志，“ x”為順時針或逆時針旋轉(zhuǎn)（ 1為順時針 2 為逆時針），“ aaa”旋轉(zhuǎn)角度，“ bbb”為速度。 本設(shè)計系統(tǒng)通訊協(xié)議為簡單通訊協(xié)議 [15]。 選擇 COM 并打開，選中要運行的運動類型并在相關(guān)位置寫上各種參數(shù)。為了更好的觀測和研究幾種運動狀態(tài)，故上位機(jī)軟件設(shè)計成各個運動狀態(tài)分開的界面。 圖 24 上位機(jī)界面 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 22 MSComm 控件是 Microsoft Communications Control 的簡稱，是 Microsoft 公司提供的簡化 Windows 下串口編程的 ActiveX 控件，它為應(yīng)用程序提供了通過串口接口收發(fā)數(shù)據(jù)的簡便方法 [14]。 上位機(jī)軟件及通訊協(xié)議 上位機(jī)軟件是利用 的 MSOCOMM 控件編寫。 發(fā)送 的數(shù)據(jù)不是一個數(shù)據(jù)，而 DSP 每次只能發(fā)送 8 位數(shù)據(jù)，故需要發(fā)送多次并檢測是否發(fā)送完畢。 無線發(fā)送子程序 無線發(fā)送子程序 流程圖如圖 23 所示 。其子程序系統(tǒng)流程圖見下圖 22所示。電機(jī)的穩(wěn)、準(zhǔn)、快的運行將會使運動方式更接近理論目標(biāo)。這樣能夠通過 TFT 液晶顯示出運行方式和參數(shù)，有利于調(diào)試程序和查找錯誤。只有三種運動狀態(tài)發(fā)生改變時此界面才變化，這是為了節(jié)省系統(tǒng)執(zhí)行時間，因彩屏執(zhí)行完此界面操作需要占用部分時間。故將三個子程序軟件系統(tǒng)流程圖 用一個表示。 標(biāo) 志 位 清 零對 接 收 參 數(shù) 值 轉(zhuǎn) 換A S C I I 碼 轉(zhuǎn) 十 進(jìn) 制上 次 旋 轉(zhuǎn) 運 動 狀態(tài) 是 否 改 變屏 幕 顯 示相 應(yīng) 運 動 界 面否是按 照 協(xié) 議 得 到各 種 參 數(shù) 值利 用 指 南 針 及 相 關(guān)公 式 得 到 三 電 機(jī) 值屏 幕 顯 相 關(guān) 參 數(shù)及 指 南 針 角 度返 回 圖 21 運動方式 子程序流程圖 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 20 本課題設(shè)計的軟件有三種運動方式，即有三種運動狀態(tài)控制子程序。 三種 基本 運動狀態(tài) 子 程序 當(dāng)接收到的數(shù)據(jù)判斷出要運行那種運動方式后，將進(jìn)入相應(yīng)子程序中 。同時電機(jī) PID 會根據(jù)設(shè)定值和誤差進(jìn)行調(diào)節(jié)，使得系統(tǒng)運行更加穩(wěn)定。 系統(tǒng)上電 后開始，系統(tǒng)初始化后開始檢測是否有接收數(shù)據(jù)，接收到之后判斷運動方式，如是停止信號則電機(jī)停止運動。其全向運動方式經(jīng)過上位機(jī)無線傳輸指令，然后 DSP 接收指令進(jìn)行處理最后控制電機(jī)協(xié)調(diào)運動，根據(jù)指南針和編碼器進(jìn)行閉環(huán)控制， 以 達(dá)到最佳效果。 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 18 通過仿真或者燒寫 FLASH 都可以在線監(jiān)控 DSP 各個寄存器和變量的值，有利于排除各種錯誤。仿真 是 將程序下載到外部數(shù)據(jù)存儲器 RAM 中，其特點是下載速度快，但掉電數(shù)據(jù)丟失，適合在開發(fā)軟件 頻繁修改 階段使用。 設(shè) 計概 念 性 規(guī) 劃 編 程 和 編 譯創(chuàng) 建 工 程 文 件編 寫 程 序 和 配 置 文 件調(diào) 試語 法 檢 測探 測 點 設(shè) 置文 件 保 存