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