機(jī)械臂三閉環(huán)伺服控制系統(tǒng)的軟件設(shè)計(jì)案例分析目錄TOC\o"1-3"\h\u19110機(jī)械臂三閉環(huán)伺服控制系統(tǒng)的軟件設(shè)計(jì)案例分析 1195231.1TMS320F28335芯片簡(jiǎn)介 1307721.2軟件框架 35061.2.1主程序設(shè)計(jì) 371751.2.2ePWM模塊配置 4182221.2.3ADC模塊配置 5152361.2.4DMA模塊配置 561631.2.5SPI與RDC模塊配置 6176311.3中斷程序設(shè)計(jì) 7270051.3.1ePWM中斷設(shè)計(jì) 7254201.3.2定時(shí)器中斷設(shè)計(jì) 9176221.4位置前饋控制程序設(shè)計(jì) 9249681.5位置模糊控制程序設(shè)計(jì) 101.1TMS320F28335芯片簡(jiǎn)介28335芯片采用176引腳LQFP四邊形封裝，其主要性能如下：（1）28335芯片采用高性能靜態(tài)CMOS技術(shù)，指令周期為6.67，主頻達(dá)到150MHz，方便電機(jī)控制，功率器件控制和工業(yè)控制。該芯片具有高性能的32位CPU。單精度浮點(diǎn)運(yùn)算單元（FPU）使用哈佛流水線結(jié)構(gòu)，可以快速執(zhí)行中斷響應(yīng)，具有內(nèi)置的內(nèi)存管理模式，并支持C/C++語言的復(fù)雜數(shù)學(xué)算法。（2）28335片上存儲(chǔ)器分為：的Flash存儲(chǔ)器，的SARAM存儲(chǔ)器，OTPROM和的BootROM。其中，F(xiàn)LASH存儲(chǔ)器以及OPTROM受系統(tǒng)密碼保護(hù)，并且用戶在這些存儲(chǔ)器中設(shè)置的程序也會(huì)受到保護(hù)。（3）28335控制時(shí)鐘系統(tǒng)具有6通道DMA控制器，片上振蕩器和看門狗模塊，支持動(dòng)態(tài)PLL調(diào)諧，內(nèi)部可編程鎖相環(huán)，并通過軟件對(duì)應(yīng)的寄存器設(shè)置CPU時(shí)鐘頻率。（4）28335的控制時(shí)鐘系統(tǒng)具有6通道DMA控制器，片上振蕩器，看門狗模塊，支持動(dòng)態(tài)PLL調(diào)節(jié)，內(nèi)部可編程鎖相環(huán)，通過軟件設(shè)置相應(yīng)寄存器的值改變CPU的輸入時(shí)鐘頻率。（5）28335內(nèi)部有16根中斷線，其中包括兩個(gè)不可屏蔽中斷與14個(gè)可屏蔽中斷。其中12個(gè)可屏蔽中斷線由8個(gè)外設(shè)中斷組成，一共支持58個(gè)外設(shè)中斷，通過外設(shè)中斷擴(kuò)展控制器（PIE）來管理片上外設(shè)和外部引腳引起的中斷請(qǐng)求。（6）28335中有增強(qiáng)型的外設(shè)模塊：18個(gè)PWM輸出，包含6個(gè)高分辨率脈寬調(diào)制模塊（HRPWM）、6個(gè)事件捕獲輸入和2通道的正交調(diào)制模塊（QEP）。（7）28335一共有3個(gè)32位的定時(shí)器，定時(shí)器0和定時(shí)器1用作一般的定時(shí)器，定時(shí)器0接到PIE模塊，定時(shí)器1接到中斷INTl3；定時(shí)器2用于DSP／BIOS的片上實(shí)時(shí)系統(tǒng)，連接到中斷INTl4，如果系統(tǒng)不使用DSP／BIOS，定時(shí)器2可用于一般定時(shí)器。（8）28335的串行外設(shè)為2通道CAN模塊、3通道SCI模塊、2個(gè)McBSP（多通道緩沖串行接口）模塊、1個(gè)SPI模塊、1個(gè)I2C主從兼容的串行總線接口模塊。12位A／D轉(zhuǎn)換器具有16個(gè)轉(zhuǎn)換通道、2個(gè)采樣保持器、內(nèi)外部參考電壓，轉(zhuǎn)換速度為80，同時(shí)支持多通道轉(zhuǎn)換。（9）此外，28335還有88個(gè)可編程的復(fù)用GPIO引腳，并且可以運(yùn)行于低功耗模式下。內(nèi)核電壓為1.9V，I/O引腳電壓為3.3V。且片上外設(shè)寄存器塊0~3只能用于數(shù)據(jù)存儲(chǔ)區(qū)，用戶不能在該存儲(chǔ)區(qū)內(nèi)寫入程序。同時(shí)，OTPROM區(qū)（0x380000~0x3803FF）為只讀空間，存儲(chǔ)A/D轉(zhuǎn)換器的校準(zhǔn)程序，用戶不能對(duì)此空間寫入程序。若設(shè)置安全代碼，存儲(chǔ)器區(qū)域Ox33FF80~0x33FFF5需全部寫入數(shù)據(jù)0x0000，而不能用于存儲(chǔ)程序或數(shù)據(jù)。反之，0x33FF80~Ox33FEF可以存儲(chǔ)數(shù)據(jù)或程序，其中0x33FFF0~Ox33FFF5只能存儲(chǔ)數(shù)據(jù)。軟件部分主要在TI公司開發(fā)的DSP芯片專用仿真平臺(tái)CCS5.5中對(duì)TMS320F28335進(jìn)行編程。1.2軟件框架1.2.1主程序設(shè)計(jì)28335的主程序主要進(jìn)行整個(gè)系統(tǒng)模塊進(jìn)行初始化工作，其中包括時(shí)鐘模塊、中斷模塊、外設(shè)模塊、IO口等等。此外還需要在主程序中定義本次實(shí)驗(yàn)中所需要的參數(shù)和變量。主程序的流程圖如下：圖4-1主程序流程圖主程序的操作流程為：首先初始化系統(tǒng)，設(shè)置28335系統(tǒng)頻率為150，禁用看門狗模塊，設(shè)置PLLCR（鎖相環(huán)電路）的系統(tǒng)時(shí)鐘頻率，設(shè)置高頻和低頻外設(shè)時(shí)鐘的預(yù)分頻器，并啟用外設(shè)時(shí)鐘；隨后初始化IO口，其中包括ePWM模塊，SPI模塊，RDC模塊和CAN模塊的初始化；第三步，關(guān)系統(tǒng)中斷，初始化系統(tǒng)的中斷向量表并且重新映射系統(tǒng)中斷服務(wù)程序的地址；下一步，初始化各個(gè)模塊配置，這一步涉及的模塊較多，將在下一條中詳細(xì)介紹；接著，初始化系統(tǒng)參數(shù)，其中主要包含電機(jī)參數(shù)、控制器參數(shù)等等；完成各項(xiàng)初始化工作后，使能部分中斷，軟件中一共用到了PWM中斷、定時(shí)器中斷、DMA中斷以及eCAN中斷，使能中斷后，開全局中斷；最后，設(shè)置一個(gè)for語句讓程序進(jìn)入無限循環(huán)中，等待中斷子程序的到來。要注意的是，在該循環(huán)語句中需要時(shí)刻判斷電機(jī)的狀態(tài)。若電機(jī)處于運(yùn)行狀態(tài)，則需要監(jiān)測(cè)電機(jī)的電流、電壓及溫度值，實(shí)時(shí)觀察電機(jī)是否有過流、過壓或過溫的情況出現(xiàn)。一旦電機(jī)進(jìn)入故障狀態(tài)，系統(tǒng)會(huì)停止輸出PWM波形，直到故障被排除。1.2.2ePWM模塊配置28335芯片中，每個(gè)ePWM模塊都是由兩路ePWM輸出組成，一共有6對(duì)這樣的ePWM模塊。由于最終希望得到六路方波輸出，因此只需要用到3對(duì)ePWM（本文使用了ePWM1,ePWM2和ePWM3）模塊。對(duì)這三個(gè)模塊進(jìn)行以下參數(shù)配置：ePWM模塊中還含有很多子模塊，軟件中主要用到了時(shí)基模塊TB、計(jì)數(shù)比較模塊CC、動(dòng)作模塊AC、死區(qū)模塊DB和事件觸發(fā)模塊ET。配置時(shí)基模塊TB。首先確定PWM的中斷頻率為10，即中斷周期為100。將時(shí)基計(jì)數(shù)器設(shè)定為向上－向下計(jì)數(shù)（增減）模式，即計(jì)數(shù)器在前半個(gè)周期中向上計(jì)數(shù)，后半個(gè)周期向下計(jì)數(shù)。計(jì)數(shù)器每1計(jì)75個(gè)數(shù)，那么一個(gè)周期時(shí)基計(jì)數(shù)器將計(jì)7500個(gè)數(shù)，其計(jì)數(shù)最大值為3750。將時(shí)基模塊設(shè)定為時(shí)基周期影子寄存器模式，每當(dāng)計(jì)數(shù)器的值歸零，周期寄存器TBPRD會(huì)立即裝載影子寄存器的值。配置計(jì)數(shù)比較模塊CC。計(jì)數(shù)比較模塊可以將時(shí)基計(jì)數(shù)器的值作為輸入，并不斷地將該值與比較寄存器CMPA和CMPB中的值進(jìn)行比較，一旦時(shí)基計(jì)數(shù)器的值等于其中一個(gè)比較寄存器的值，就會(huì)產(chǎn)生相應(yīng)的事件。采用比較寄存器CMPA來控制PWM的占空比，每當(dāng)SVPWM輸出不同的，系統(tǒng)就會(huì)相應(yīng)地更新CMPA的值。CMPB設(shè)置為1500，即在20時(shí)時(shí)基計(jì)數(shù)器的值等于CMPB的值。比較模塊也設(shè)置了影子寄存器，和TB模塊的一樣，也是過零裝載。配置動(dòng)作模塊AC。當(dāng)ePWM模塊產(chǎn)生相關(guān)事件時(shí)，動(dòng)作模塊會(huì)決定是輸出高電平還是低電平。由于需要利用CMPA來控制系統(tǒng)占空比，因此將CMPA的動(dòng)作設(shè)定為：向上計(jì)數(shù)時(shí)，當(dāng)時(shí)基計(jì)數(shù)器的值等于CMPA的值，ePWMxA輸出高電平；向下計(jì)數(shù)時(shí)，發(fā)生同上事件后，ePWMxA輸出低電平。配置死區(qū)模塊DB。ePWM模塊中的三相全橋逆變電路很容易造成短路的問題。因此，設(shè)置死區(qū)時(shí)間為20，使能上升沿下降沿雙邊延時(shí)，并且設(shè)ePWMxA輸出極性翻轉(zhuǎn)，ePWMxB輸出極性不翻轉(zhuǎn)。配置事件觸發(fā)模塊ET。系統(tǒng)每產(chǎn)生一個(gè)事件就會(huì)產(chǎn)生一個(gè)ePWM中斷，并設(shè)置ePWM1模塊在時(shí)基計(jì)數(shù)器為其最大值時(shí)觸發(fā)事件1（ePWM1中斷），ePWM2模塊在計(jì)數(shù)器值等于CMPB時(shí)觸發(fā)事件2（ePWM2中斷）。1.2.3ADC模塊配置ADC模塊使用內(nèi)部參考電壓，時(shí)鐘頻率為25，采樣頻率為12.5，即每0.08進(jìn)行一次采樣。設(shè)置ADC模塊的2個(gè)8狀態(tài)排序器獨(dú)立工作，兩個(gè)排序器按照順序一一進(jìn)行采樣。排序器工作在啟動(dòng)/停止模式下，每當(dāng)有觸發(fā)信號(hào)到來時(shí)，排序器才會(huì)開始排序工作。設(shè)定排序器的采樣順序?yàn)椋旱谝淮螌?duì)通道3進(jìn)行采樣，可以得到的值；第二次對(duì)通道1進(jìn)行采樣，可以得到的值；第三次對(duì)通道10進(jìn)行采樣，可以得到的值。1.2.4DMA模塊配置DMA模塊可以將ADC模塊轉(zhuǎn)換完的電流結(jié)果傳送到相應(yīng)的地址中。DMA模塊一共有6個(gè)通道，使能DMA通道1來進(jìn)行數(shù)據(jù)交換工作。DMA通道的設(shè)置必須與ADC設(shè)置的采樣次數(shù)及轉(zhuǎn)換次數(shù)相匹配。設(shè)源地址為ADC模塊的結(jié)果寄存器0，目標(biāo)地址為DMA模塊的緩沖區(qū)。首先配置DMA內(nèi)循環(huán)，讓DMA從結(jié)果寄存器0開始傳送3個(gè)數(shù)據(jù)，由于每個(gè)數(shù)據(jù)會(huì)傳送5次，因此每傳完一個(gè)數(shù)據(jù)，目標(biāo)地址需要向后移5位，為接下來的四次數(shù)據(jù)傳輸留下空間。而源地址向后移一位，繼續(xù)傳送下一個(gè)結(jié)果寄存器的值。再配置DMA的外循環(huán)，第一次內(nèi)循環(huán)結(jié)束后，目標(biāo)地址一共后移了10位，源地址后移了2位，所以要將源地址前移2位回到結(jié)果寄存器0。由于原來的目標(biāo)地址已經(jīng)寫過數(shù)據(jù)了，因此需要對(duì)該地址的下一位傳送數(shù)據(jù)，所以目標(biāo)地址要前移（10-1）位，回到上一次數(shù)據(jù)傳輸中的第一位地址的下一位。通道1的數(shù)據(jù)傳送工作通過SEQ1（排序器1）中斷觸發(fā)，工作在連續(xù)模式下，并且在傳送結(jié)束后會(huì)產(chǎn)生DMA中斷，該中斷會(huì)告知CPU數(shù)據(jù)傳送工作已完成。使用DMA模塊的好處在于，DMA模塊能夠用硬件實(shí)現(xiàn)存儲(chǔ)器與存儲(chǔ)器之間、存儲(chǔ)器與IO設(shè)備之間的高速數(shù)據(jù)傳送，這個(gè)過程中不需要CPU參與，大大減少了CPU的操作負(fù)擔(dān)。而且存儲(chǔ)器地址的修改和傳送均由硬件自動(dòng)完成，極大程度地提高了數(shù)據(jù)傳輸速度和效率，減少了程序耗時(shí)。1.2.5SPI與RDC模塊配置SPI模塊工作在主/從模式下，其中28335芯片為主控制器，RDC模塊（旋變解碼芯片）為從控制器。設(shè)置SPI模塊通訊速率為低速時(shí)鐘的5分頻，即。由于RDC模塊內(nèi)部設(shè)為8位數(shù)據(jù)字長(zhǎng)，所以將SPI模塊相應(yīng)地設(shè)置一次可傳輸8位數(shù)據(jù)長(zhǎng)度。讀數(shù)據(jù)時(shí)，SPI模塊采用查詢方法，可以主動(dòng)發(fā)出信號(hào)來查詢旋變解碼芯片中的數(shù)據(jù)。SPI模塊主要的通訊任務(wù)就是進(jìn)行角度信號(hào)與轉(zhuǎn)速信號(hào)的讀取，均在ePWM2中斷內(nèi)進(jìn)行。對(duì)于RDC模塊來說，使用AD2S1210芯片可實(shí)現(xiàn)旋變解碼電路。設(shè)該芯片工作模式為配置模式，激勵(lì)頻率為10，14位分辨率，最大可跟蹤速率為。AD2S1210芯片的位置和速度數(shù)據(jù)分別儲(chǔ)存在地址0x80-0x83中。SPI模塊首先從0x80中讀取位置數(shù)據(jù)，同時(shí)會(huì)發(fā)送一個(gè)數(shù)據(jù)。此時(shí)發(fā)送的數(shù)據(jù)并不是剛剛讀到的位置值，而是將之前存儲(chǔ)在SPI寄存器中的數(shù)據(jù)發(fā)送了出去。而這一個(gè)周期讀到的位置數(shù)據(jù)要在下一個(gè)周期發(fā)送。因此，若需要讀取一次轉(zhuǎn)速或位置信息，必須讓SPI進(jìn)行三次讀取與傳輸工作，這樣才能得到一個(gè)完整的轉(zhuǎn)速/位置數(shù)據(jù)。讀取角度時(shí)主要會(huì)進(jìn)行以下操作：首先更新AD2S1210中的位置與速度信息，隨后利用SPI模塊與RDC模塊進(jìn)行通訊，獲取角度信息。由于SPI模塊要讀取兩次角度數(shù)據(jù)，一次讀取8位，而AD2S1210的分辨率為14位，因此要將讀到的數(shù)據(jù)右移兩位來確保數(shù)據(jù)的準(zhǔn)確性。隨后，需要通過換算處理將該14位數(shù)據(jù)轉(zhuǎn)換為實(shí)際的角度值。讀取速度值同理。但需要注意的是，SPI模塊讀取到的16位速度數(shù)據(jù)最高位代表轉(zhuǎn)速方向，1代表正轉(zhuǎn)，0代表反轉(zhuǎn)。若為反轉(zhuǎn)，需要對(duì)該數(shù)據(jù)取補(bǔ)碼才能得到真實(shí)的轉(zhuǎn)速數(shù)據(jù)。具體流程如下圖所示：圖4-2SPI模塊程序流程圖1.3中斷程序設(shè)計(jì)1.3.1ePWM中斷設(shè)計(jì)28335芯片內(nèi)部可以產(chǎn)生多種中斷信號(hào)，本程序中主要用到了ePWM中斷和定時(shí)器中斷。ePWM中斷作為本次程序中的主中斷，中斷周期設(shè)置為100，并主要執(zhí)行以下操作：讀取電流、轉(zhuǎn)速、位置信號(hào)，監(jiān)測(cè)電機(jī)與控制器的實(shí)時(shí)狀態(tài)，并實(shí)現(xiàn)對(duì)電流環(huán)、轉(zhuǎn)速環(huán)和位置環(huán)的三閉環(huán)控制。圖4-3ePWM中斷設(shè)置將ePWM中斷分為兩個(gè)子中斷：ePWM1中斷和ePWM2中斷。EPWM1中斷在中斷周期中點(diǎn)處觸發(fā)，主要負(fù)責(zé)電流環(huán)的相關(guān)操作。進(jìn)入ePWM1中斷后，首先會(huì)采集電機(jī)的電流值，此時(shí)采到的電流值為靜止坐標(biāo)系下的電流值。由于最終需要將該電流值反饋到電流環(huán)中，必須經(jīng)過Clark變換以及Park變換后轉(zhuǎn)換為d-q坐標(biāo)系下的電流值。得到反饋電流值之后，便需要對(duì)電流環(huán)進(jìn)行計(jì)算，此時(shí)系統(tǒng)會(huì)自動(dòng)判斷現(xiàn)在的閉環(huán)情況。若系統(tǒng)正處于位置三閉環(huán)或者轉(zhuǎn)速雙閉環(huán)中，那么電流環(huán)的輸入給定就等于速度調(diào)節(jié)器的輸出值；若系統(tǒng)直接處在電流環(huán)中，那么此時(shí)相當(dāng)于沒有轉(zhuǎn)速環(huán)和位置環(huán)控制，需要上位機(jī)來給定。給定和反饋值都得到之后，開始進(jìn)行電流環(huán)的計(jì)算，首先算出電流調(diào)節(jié)器的輸出值，再通過反Park變換及SVPWM算法，在下一個(gè)周期更新比較寄存器CMPA的值，改變占空比，輸出相應(yīng)的六路方波來控制電機(jī)輸出新的電流值，該電流值經(jīng)過Clark變換和Park變換后作為電流環(huán)的反饋電流。最后還需要進(jìn)行過流保護(hù)，再重復(fù)以上操作。ePWM2中斷主要進(jìn)行角度讀取和轉(zhuǎn)速讀取工作，一般會(huì)花費(fèi)20左右時(shí)間，運(yùn)行時(shí)間較長(zhǎng)，因此選擇在時(shí)觸發(fā)中斷。該中斷的讀取角度和轉(zhuǎn)速的具體操作在5.1.5中已經(jīng)詳細(xì)做過介紹，故不再贅述。1.3.2定時(shí)器中斷設(shè)計(jì)相較于電流環(huán)，轉(zhuǎn)速環(huán)和位置環(huán)的計(jì)算不需要ePWM中斷那樣大的中斷頻率，因此將這兩個(gè)環(huán)的相關(guān)操作放在了定時(shí)器中斷內(nèi)。將定時(shí)器中斷頻率設(shè)置為1，中斷周期為1。定時(shí)器中斷內(nèi)會(huì)對(duì)位置環(huán)與轉(zhuǎn)速環(huán)進(jìn)行PI調(diào)節(jié)器運(yùn)算，最終目的都是獲得速度調(diào)節(jié)器的輸出來作為電流環(huán)的輸入。首先系統(tǒng)仍然要判斷現(xiàn)在系統(tǒng)的閉環(huán)情況，但此時(shí)只需判斷是位置三閉環(huán)還是轉(zhuǎn)速雙閉環(huán)。若是轉(zhuǎn)速雙閉環(huán)，此時(shí)不存在位置環(huán)，所以轉(zhuǎn)速輸入需要通過上位機(jī)給定，反饋轉(zhuǎn)速為旋變解碼芯片中的速度值，之后便直接進(jìn)行速度環(huán)PI計(jì)算即可得到轉(zhuǎn)速輸出。若為位置環(huán)，此時(shí)將ePWM2中得到的角度值換算成實(shí)際的負(fù)載端角度信息，作為反饋角度。隨后依次進(jìn)行位置調(diào)節(jié)器和速度調(diào)節(jié)器的計(jì)算，最后獲得轉(zhuǎn)速輸出值。1.4位置前饋控制程序設(shè)計(jì)在5.1節(jié)和5.2節(jié)中主要完成了主程序和中斷程序的設(shè)計(jì)，實(shí)現(xiàn)了機(jī)械臂的永磁同步電機(jī)傳統(tǒng)三閉環(huán)伺服控制系統(tǒng)。若要加入位置前饋控制，只需要在5.2節(jié)中所提到的兩個(gè)中斷程序中加上位置前饋控制的內(nèi)容即可。位置前饋控制的主要程序放在定時(shí)器中斷中。若系統(tǒng)判定目前正處于位置前饋控制系統(tǒng)中，便會(huì)執(zhí)行前饋控制的相關(guān)操作。前饋控制程序會(huì)涉及到位置正弦給定曲線的生成程序。給定正弦曲線的頻率以及幅值由上位機(jī)進(jìn)行給定，程序會(huì)自動(dòng)計(jì)算出給定曲線的角速度。而給定正弦幅值為機(jī)械臂的角度信號(hào)，所以還需要換算為電機(jī)的旋轉(zhuǎn)圈數(shù)。實(shí)現(xiàn)前饋控制，只需要在該子程序中計(jì)算出前饋幅值即可，根據(jù)3.2節(jié)推導(dǎo)出轉(zhuǎn)速前饋幅值和電流前饋幅值的計(jì)算公式： （42） （43）其中，為轉(zhuǎn)速前饋幅值，為轉(zhuǎn)速前饋的衰減系數(shù)，為正弦給定信號(hào)的幅值，為正弦給定信號(hào)的角速度，乘以60代表的單位換算。為電流前饋幅值，為電流衰減系數(shù)，為電流與轉(zhuǎn)矩之比。隨后直接將生成的位置正弦給定曲線作為位置調(diào)節(jié)器的輸入進(jìn)行計(jì)算，再將輸出的轉(zhuǎn)速值與前饋轉(zhuǎn)速值相加后作為轉(zhuǎn)速調(diào)節(jié)器的輸入，繼續(xù)進(jìn)行轉(zhuǎn)速環(huán)的計(jì)算。最后，在定時(shí)器中斷中會(huì)直接對(duì)電流前饋值進(jìn)行處理，將正弦給定子程序中計(jì)算得到的前饋電流與轉(zhuǎn)速環(huán)的輸出電流值相加，得到電流給定之和。對(duì)ePWM1中斷進(jìn)行修改。同樣，若系統(tǒng)判斷目前處在前饋控制系統(tǒng)中，便會(huì)將定時(shí)器中斷中計(jì)算出的之和作為電流環(huán)的給定，之后再進(jìn)入電流環(huán)的運(yùn)算程序。1.5位置模糊控制程序設(shè)計(jì)模糊控制程序只需要修改定時(shí)器