伺服机构韧体系统与控制电路开发--毕业论文.doc

《伺服机构韧体系统与控制电路开发--毕业论文.doc》由会员分享，可在线阅读，更多相关《伺服机构韧体系统与控制电路开发--毕业论文.doc（80页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、第一章、導論 伺服機構理論(Servomechanism Theory)起源於二次世界大戰期間，美軍發展控制機械系統的閉迴路控制技術，以開發具有自動控制功能的雷達追蹤系統，此一發展奠定了後來伺服機構理論的基礎。一個現代的伺服系統的設計包含了機械設計、馬達控制、電力電子、伺服控制、運動控制、程式設計、雜訊防制、實務應用等技術與經驗，其核心技術在於整合微電子與電力電子技術實現伺服控制技術。1-1 研究動機 在科技迅速的發展之下，以往的人操作已漸漸被機械手臂所取代，在現代的工業已成為可或缺的機器，在工業之外，機械手臂也廣泛的應用於週遭生活上。隨著微機電系統、電力電子技術的發展，伺服技術則進一步結合微

2、電子與電力電子技術，以韌體控制的方式呈現，而微處理器及積體電路的不斷進化，伺服技術的發展也將朝向單晶片控制、智慧控制的方向發展。 本人對於電子電路及軔體程式設計深感興趣，在指導教授黃敏昌博士的帶領下，有幸踏入機械手臂與機器人領域發揮個人所學，期望能在伺服控制系統的韌體程式及電子電路設計上有所收穫。1-2 研究目的 本論文以五軸機械手臂為基礎，以微處理器晶片開發伺服控制韌體程式，同時設計一套完整之控制電路，於實作中不斷改進，達到機電整合之目標。並且以模組化導向，進一步改寫軔體程式及開發邏輯電路，適用於不同機械手臂與機器人架構。1-3 論文架構本論文主要分為六個章節及一個附錄，茲將各章分述如下：第

3、一章、 緒論。第二章、 五軸機械臂系統電路設計。第三章、 五軸機械臂軔體程式設計。第四章、 高階語言模組化韌體程式。第五章、 兩相編碼器解碼邏輯電路。第六章、 結論。附錄、微處理器開發環境及組合語言介紹。第二章、五軸機械臂系統電路設計2-1 五軸機械臂五軸機械手臂各軸傳動機構配置包括驅動馬達、減速齒輪組與旋轉編碼器。第一軸至第三軸除了驅動馬達外還設計一組總輸出達到1:300的減速齒輪組，並安裝兩相旋轉編碼器作為閉迴路控制的回授訊號源。第四軸與第五軸則使用內建有減速齒輪與旋轉編碼器的馬達組件。第二軸因承載最大的負荷，故規劃兩顆馬達同時驅動，且需兩組同步作動之驅動電路。其各軸詳細規格與作動流程整理

4、如表2.1及圖2.1所示。表2. 1 各軸傳動機構配置圖2. 1 機構作動流程2-2 電路設計 五軸機械臂的系統電路包含控制面板、微處理器電路、光耦合晶片、馬達驅動電路與旋轉編碼器所組成的直流馬達閉迴路控制系統(圖2.2)。每一區塊各自獨立，可因應不同的機構做電路上的修改與刪減，對於系統開發而言具有很大的彈性。圖2.2 系統電路 本章著重於電子電路之設計與實際製作過程，分節說明各區塊電路與IC晶片的主要功能。至於各區塊及IC訊號間的完整運作模式，則留待第三章再一併深入探討。2-2-1 控制面板 控制面板的作用是對直流馬達下達前進、後退、停止等指令，讓所操縱的機械手臂轉軸能做相應的運動模式。面板

5、的控制方式可以用按鍵、轉軸、操縱桿或是連結到個人電腦透過應用程式下達指令，雖然控制方式有所差異，但對伺服控制處理核心而言卻是大同小異。 本次實驗設計兩種控制方式，第一種是用兩個按鍵控制馬達的正反轉，目的在使機械手臂能以最直覺的方法控制，通常應用於系統開機時手動校正各轉軸位置。第二種是使用可變電阻當軸心，旋轉可變電阻時機械手臂能做出相對應的運動，目的使各轉軸皆能正確的轉動到可變電阻所對應的角度，實現伺服控制的目標。外加一按鍵可切換兩種不同操作模式，以及一個電源開關與LED指示燈。圖2.3是單軸控制面板的電路設計圖，其中J1的Pin1與Pin7連接到5伏特電壓(VCC)與接地(GND)提供微處理器

6、的工作電源，並在VCC端接上一個切換開關SW1。J1 Pin6為LED指示燈(D1)與220歐姆限流電阻(R2)，連接至微處理器。圖2.3 控制面板電路 J1 Pin4、3、2分別為設定、前進、後退等按鍵，對於微處理器的數位輸入模式而言只有高電位與低電位兩種類型，程式運作過程中若發生輸入訊號無法辨識或訊號浮動(Floating)情形，則會出現不可預期的錯誤。因此在控制按鍵電路中，需加一個10K歐姆的接地電阻(R3、R4、R5)避免這問題。 10K旋轉可變電阻(R1)有3個接腳，上下接腳分別連接至3.3V與GND，中間接腳則連接至微處理器的類比數位轉換模組(A/D Converter)輸入接腳。

7、當可變電阻上下兩段的電阻值改變，利用電路分壓定理即可求出中間接腳的電壓大小，如圖2.4。不同的類比電壓經過A/D Converter之後對應出不同的數位值，微處理器依據這些數值，判斷機械手臂需如何轉動及轉動多少角度。圖2.4 可變電阻分壓定理2-2-2 微處理器電路 微處理器是整個伺服控制系統的核心，此部分電路除了與光耦合晶片連接的馬達控制信號及旋轉編碼器信號以3 PIN排線連接之外，其餘各項I/O控制信號與電源皆連到控制面板上。微處理器的主電源與旋轉編碼器電源使用5V直流電壓，而控制面板按鍵、LED、與可變電阻控制軸是採用微處理器提供的3.3V邏輯電壓。本次實驗採用義隆電子單晶片EM78M6

8、11微處理器，各輸出入腳位設定與電路設計如表2.2及圖2.5所示，微處理器的詳細功能及介紹可參考附錄。表2.2 微處理器使用腳位圖2.5 微處理器電路2-2-3 旋轉編碼器 直流馬達的閉迴路控制必須藉由旋轉編碼器的回授信號才能達成伺服控制的目的，其功能為偵測轉動狀態並輸出相對應之數位訊號。本論文五軸機械手臂採用增量型霍爾磁感應式編碼器(OMRON E6A2-CW3E 圖2.6)以及內含編碼器的直流伺服馬達。圖2.6 旋轉編碼器OMRON E6A2-CW3E 圖2.7 霍爾IC 編碼器中主要電子元件霍爾IC的構造如圖2.7，利用電子學霍爾效應感應轉軸上之磁場變化。元件特性為開集極型輸出低電位與浮

9、動輸出(Floating)，因此輸出端需加入提升電阻使其具備高低電位的數位模式。2-2-4 馬達驅動電路 一般數位電路系統受限於電功率問題，無法直接驅動直流馬達，通常會採用驅動晶片做為大功率馬達的驅動模組。本實驗採用Allegro MicroSystem, Inc.的A3952SB全橋驅動晶片，主要原因是該晶片能提供最大2安培的驅動電流符合機構的需求。參考DataSheet規格，將驅動晶片的主電源及控制信號準位設計成使用57V直流電壓，而提供馬達的輸出電源則採用12V或24V直流電壓。圖2.8 A3952SB規格圖2.9 驅動晶片A3952SB參考電路 雖然晶片額定輸出能提供2安培的驅動電流，

10、但還是需要靠電路選擇最大輸出容許電流(ITRIP)，此方法是依據Pin2的輸入電壓參考值(VREF)及Pin11的接地電阻值(RS)決定，ITRIP=VREF/(10 RS)。圖2.9的參考電路ITRIP為1安培，依此電路應用於五軸機械臂，會發現其中負載較大的轉軸常常有無法轉動之現象，此即為ITRIP不足之結果。故於最終電路設計上修改VREF為6伏特、RS為0.33歐姆，則ITRIP可提升至1.8安培，讓五軸機械臂正常運轉。驅動晶片的主要控制信號為MODE、ENABLE、PHASE與BRAKE，驅動模式如真值表(表2.3)。在本電路實際應用中將MODE接地並控制其他三個信號的高低準位來驅動馬達

11、，控制方法如表2.4。表2.3 控制信號真值表表2.4 控制信號實際應用方式2-2-5 光耦合晶片 馬達驅動晶片相較於數位電路而言是一個大功率電子元件，當馬達在啟動和停止的瞬間甚至是運轉中的轉速變化，會產生很大的脈衝電流及突波電壓，必需將驅動電路與數位微處理器做適當的隔離，避免不必要的雜訊干擾造成系統失效。本系統驅動電路與數位電路分別採用獨立的電源，其接地端也各自獨立，並透過PC817光耦合晶片作為控制信號的中繼轉換。光耦合晶片是由發光二極體與光電晶體組成之開關電路，能夠避免兩獨立電路互相干擾。且能用於改變數位信號的電壓準位，使其能適用於不同數位電路。圖2.10 光耦合電路 圖2.10中，V1

12、、V2為獨立電源，R1、R2為限流電阻。當Input為高電位時二極體不導通，電晶體亦不導通，故Output為高電位；當Input為低電位時二極體導通，電晶體亦導通，故Output為低電位。圖2.11為整合馬達驅動晶片與光耦合晶片的連接方式及電路設計。J1、J2提供驅動晶片參考電壓與主電源，接地獨立不與微處理器電源連接。光耦合晶片左側連接微處理器控制信號並以LED作為信號高低準位的指示燈，燈亮代表控制信號為LOW。光耦合晶片右側將控制信號連接驅動晶片，並以提升電阻作為高準位的參考電壓。圖2.11 光耦合及驅動晶片電路2-3 泛用型驅動電路板 為了證實本章提出之電路設計確實可行，綜合本章所有電路區

13、塊並以五軸機械手臂為實驗標的，以萬用電路板手焊拉線方式製作初步實驗用電路成品。因為要同時控制機械手臂的五個軸，將所有微處理器電路與驅動晶片電路集中在同一個電路板上，並且製作具有五個面板的控制盒來操控，電路成品如圖2.12、圖2.13。圖2.12 五軸核心電路成品圖圖2.13 五軸控制面板成品圖在指導教授黃敏昌博士的帶領下，使用這套電路參加第十屆義隆盃微控制器應用製作大賽，證明伺服控制電路確實可行，但在製作過程中有些難題無法克服。電路板需手工焊接許多跳線，難免有些地方不牢固容易脫落，跳線電路複雜，發生問題需耗費許多時間除錯，甚至可能找不到真正的原因。為了解決上述問題，決定將電路製作成印刷電路板形

14、式，擷取本章電路精華，設計適用於不同條件下的泛用型驅動電路，使其能提供未來開發機械手臂或機器人使用。2-3-1 電路設計及製作 泛用型驅動電路應能適用於不同核心處理單元及不同直流馬達特性。電路需具備輸入端子以接收數位形式信號，此端子可連接至微處理器、可程式邏輯單元或其他數位晶片，作為伺服控制下達命令的核心。其次為控制直流馬達的輸出端子，如前所述，電路中必須採用驅動晶片才能驅動大功率直流馬達，且需以光耦合晶片隔離數位信號。而回授功能則採用旋轉編碼器，並連接至核心處理單元。完整電路繪製如圖2.14。圖2.14 泛用型驅動電路圖 為改善傳統手焊電路板缺點，本電路被動元件使用表面黏著式元件(Surfa

15、ce-Mount Device，SMD)，該類型元件的優點可概括如下幾點：1. 表面黏著式元件在電子線路設計上較為快捷，而且能減少線路在運作上之互相干擾。 2. 表面黏著式元件其體積細小，所以對比於插裝元件擺放在電路板上所佔之面積可大為縮小，可減小電路板面積及成本。 3. 表面黏著技術在電子線路上亦能大為改善其線路運作性能，特別是對於一些高頻模擬線路、數位線路、高噪音和微波線路等。圖2.15 表面黏著元件(SMD 0805)SMD電阻或電容依其封裝大小有0603、0805、及1204等規格，本電路板除了大功率水泥電阻及穩壓電容外，所有被動元件皆採用SMD 0805規格。此一改變至少能大幅減少電

16、路板面積，且電路的連接也更容易辨識。其餘IC元件及連接器等，考量本身規格限制及外接訊號的便利性仍採用插件方式，完整零件配置如表2.5所示。表2.5 泛用型驅動電路零件表透過Protel軟體將電路圖製作成電路板的Layout圖面，除了將上述所有零件擺放於適當位置，各零件的電路亦需調整適當的走線，目的使電路板面積縮小且能兼顧使用便利及易於除錯辨識。最終設計將泛用型驅動電路板製作成長80mm、寬40mm、厚度1.6mm的雙面電路板。其零件擺放位置及正反面走線如圖2.16與圖2.17所示。圖2.16 零件擺放位置圖2.17 正反面走線因泛用型驅動電路板屬於雙面製程，且需製作上百片之數量供未來實驗使用。

17、若以感光電路板自行蝕刻，花費之時間與成本難以估計。因此將設計完成之電路圖及Layout圖面檔案委外製作成印刷電路板，不僅能節省成本且品質更為精良。圖2.18、2.19即為泛用型驅動電路板完整成品。圖2.18 印刷電路板成品圖2.19 泛用型驅動電路板成品2-3-2 使用介紹 將光耦合晶片(PC817)與馬達驅動晶片(A3952SB)分別插入U2、U3腳座，並依各連接器腳位連接正確之訊號。圖2.20 電路板連接器標號 J1功能為輸出驅動電流至直流馬達，其次為連接編碼器提供作動所需之電源及接收回授訊號。表2.6 J1腳位說明 如2-2-3節所述，編碼器主要電子元件霍爾IC會產生浮動輸出現象(Flo

18、ating)，因此輸出端需加提升電阻於本電路板R9與R10。某些編碼器本身已經內建提升電阻，為適用於不同類型編碼器，電路中設計SW1與SW2供選擇是否使用電路板的提升電阻。 J3連接至核心處理單元，只要依照腳位說明接上正確之訊號，無論使用何種數位晶片皆能運作無誤。表2.7 J3腳位說明 電源接頭(J2、J4、J5)分別為Logic 5V、6V與24V電源，其中Pin 1為接地端(GND)，Pin 3為電源端(VCC)。J2 Logic 5V與J3 Pin 3、4相通，選擇其中一組即可，於此電路中主要功能為提供旋轉編碼器(J1 Pin 6、7)及光耦合晶片使用。J4(6V)為馬達驅動晶片作動電源

19、及驅動訊號之參考電壓。J5(24V)提供馬達傳動之電源，於此電路中亦可依馬達類型使用較低伏特之電源。第三章、五軸機械臂軔體程式設計 本章依據微處理器EM78M611架構，開發一套適用於五軸機械臂系統的組合語言程式，利用參數化設計技巧，只需改變幾個參數就能符合不同之需求。目的使第二章電路系統及機械手臂裝置能正常運作以實現閉迴路伺服控制的目標。3-1 驅動晶片控制信號微處理器針對馬達驅動晶片發出三組控制信號(ENABLE、PHASE、BRAKE)，ENABLE控制驅動晶片是否作動，當訊號為低準位時晶片啟動中，訊號為高準位時晶片處於休眠模式無法作動。因此在控制過程中，ENABLE訊號需保持在低準位。

20、BRAKE可視為馬達啟動與停止的訊號，BRAKE為高準位時馬達啟動，BRAKE為低準位時馬達停止。PHASE代表馬達正反轉方向，欲改變PHASE的方向必須在馬達停止的狀態(BRAKE=1)。因此在控制馬達的過程中需遵循圖3.1的流程，以避免不可預期之錯誤。圖3. 1 驅動晶片控制信號3-2旋轉編碼器信號解讀五軸機械臂的回授裝置採用OMRON增量型旋轉編碼器，其額定規格如表3.1。此兩相編碼器除了可以測出運轉的速度、位置外，A、B兩相電晶體在排列上差了90度相位差，因此觀察A、B訊號波形的相位前後次序即可知道運轉的方向。表3. 1 增量型旋轉編碼器圖3. 2 兩相編碼器輸出波形兩相編碼器解碼過程

21、需透過一連串之邏輯運算，在實務上常使用邏輯電路或可程式邏輯晶片(Programmab1e Logic Device)做為解碼電路的設計工具，開發過程將於第五章深入探討。本章單純採用微處理器程式解碼，因解碼過程需龐大的程式執行週期，考量到程式運算速度及馬達轉速，無法同時兼顧系統其餘程式區塊，因此只使用單相編碼器訊號作為微處理器程式的解碼依據。此方式於每一特定時間內讀取編碼器的輸出準位並比較前後準位是否改變，在一個轉動週期內會出現兩次準位改變(脈衝訊號)，只要時間間隔夠短將可正確的得到這些資訊。反之若時間間隔太長，程式執行過程將不斷漏失許多脈衝訊號。以本次實驗五軸機械臂為例，表3.2藉由各軸馬達轉

22、速、減速齒輪與旋轉編碼器解析度求出編碼器輸出訊號的最短週期。表3. 2 各軸最短週期程式每次讀取編碼器訊號的間隔時間以不超過四分之一最短週期為原則，由上表得到約為250S。EM78M611晶片個指令周期需要2個振盪頻率，若以12MHz時脈運作下，個指令週期所需時間為0.167S。因此程式每執行1500次指令以內就必須讀取一次編碼器訊號，才能確保每次脈衝訊號都能有效讀取。3-3 類比數位轉換模組 本系統人機介面使用旋轉可變電阻改變類比電壓大小，經由類比數位轉換模組告知微處理器一組目標命令數值(參考第2-1節)。微處理器EM78M611內建類比數位轉換器(A/D Converter)，類比信號輸入

23、腳位由控制暫存器(ERA)控制5位元的多工器，選擇特定之腳位。並且透過ERD切換不同的執行速率，轉換完成之10位元數位資料存放於暫存器ERB與ERC。使用轉換模組前必需先設定控制暫存器IOCA的bit 3，才能對A/D Converter控制暫存器進行設定。表3. 3 類比數位轉換控制暫存器 轉換流程如圖3.3，實際轉換程序所需時間由轉換速率決定，在此過程中微處理器可執行其他程式碼無須等待。完成後可由ERB、ERC得到轉換之結果。圖3. 3 轉換流程3-4 軔體程式架構 完整程式包含下列數個主要區塊且將這些區塊設計為副程式，增加程式的可利用性及可讀性馬達歸位模式(SET_MOTOR)電源開啟初

24、期，以控制面板上的按鍵控制馬達前後轉動，將機構運轉至適當的位置。解碼模式(DECODER)讀取旋轉編碼器訊號並解碼計數，得到馬達目前的位置。輸入目標資訊(INPUT)讀取經過A/D轉換後的控制面板輸入資訊，設定馬達轉動的目標。比較模式(COMPARE)比較馬達轉動目標及目前位置，做為下一步動作的判斷依據。下達命令(COMMAND)對各軸下達指令，前進、後退、停止。上述副程式區塊及完整軔體流程如圖3.4。圖3. 4 完整軔體流程3-4-1 副程式SET_MOTOR 電源開啟初期，以控制面板上的按鍵控制馬達前後轉動，將機構運轉至適當的位置，做為機械手臂機構初始化的設定模組。目的讓往後操控可變電阻旋

25、扭時，各軸機構的旋轉角度能有足夠的運動空間。控制面板上有設定(SET)、正轉(FW)及反轉(BW)三個按鍵，分別以P6.5、P6.6與P6.7為其輸入腳位，按下時輸入高準位、放開時輸入低準位。按下SET按鍵則代表完成設定模式，程式離開SET_MOTOR區塊，進入伺服控制模式。圖3. 5 副程式SET_MOTOR流程圖表3. 4 副程式SET_MOTOR程式碼 由於一般按鍵都屬於機械式按鍵開關，當按下按鍵時免會因為機械的簧片回振盪，而產生所謂的彈跳效應(圖3.6)，此現象會造成軔體程式誤判而使系統失效或誤動作。一般按鍵彈跳效應都會在510mS內結束而趨於穩定，在此時間內消除彈跳的方法有很多種，但

26、可分為硬體及軔體消除彈跳二大。硬體方面可採用正反器、鎖相迴等位輯電消除。而本章採用軔體程式設計方法，排除彈跳效應可能造成之影響，當偵測到按鍵電輯改變時，刻延遲30mS後再一次偵測訊號準位，當訊號真正改變時再執下一步工作。選擇延遲30mS的原因為人類按鍵動作的反應時間不會低於30mS，除了可以避免彈跳效應外又不會漏失真正的按鍵動作。圖3. 6 彈跳效應 副程式SET_MOTOR中的DELAY_30mS即是消除彈跳效應的延遲時間副程式，以迴圈的方式設計程式碼如表3.5。表3. 5 副程式DELAY_30mS3-4-2 副程式DECODER 副程式DECODER作用為讀取來自旋轉編碼器訊號SENSO

27、R (P7.0)，經過解碼後更新計數器(COUNTER)數值。此計數器由兩個暫存器組成(COUNT1、COUNT0)，因此最多可計數至216。解碼後的單相訊號只能得到脈衝訊號之數量，無法得知運轉方向，因此在程式中預先定義一組代表馬達轉動狀態之旗標(SOM)，解碼過程中參考SOM的值判斷計數器的方向。SOM旗標共有五種狀態如表3.6。其中慣性正轉與慣性反轉代表程式下達停止指令之後，馬達本身的轉動慣性使馬達持續運轉一小段時間才完全停止，在這段過程中必須持續計數以得到馬達正確的位置。表3. 6 旗標SOM程式中定義一個靜止旗標(STATIC)判斷馬達是否處於靜止狀態，當旋轉編碼器SENSOR(P7.

28、0)訊號重複出現則STATIC數值加1，若訊號改變則STATIC歸零。若STATIC數值大於某一個預先設定的值，即可認定馬達已經完全靜止。DECODER完整說明如圖3.7及表3.7。圖3. 7 副程式DECODER流程圖表3. 7 副程式DECODER程式碼3-4-3 副程式INPUT 用於解讀伺服控制的輸入指令，並將之轉化為具有上下限的控制區間。本次實驗控制面板採用可變電阻旋轉軸改變類比電壓大小，經過微處理器內建A/D Converter模組轉換成一組8位元數值(0255)，將此數值換算後分別對應於適當的編碼器計數值與機構轉動角度。機構中不同的齒輪比、編碼器設計及轉動範圍的限制，必須採用不同

29、的轉換倍率，以本實驗的五軸機械臂為例，其對應如表3.8所示。表3. 8 五軸機械臂各軸旋轉角度 副程式中以TIMES、LorR、WINDOW參數數值大小決定轉換倍率與上下限(表3.9說明其意義)，並在程式宣告區設定三個常數(adset1、adset2、adset3)分別代表之，方便不同轉軸的設定。其中WINDOW的大小會影響機構轉動到達目標區間至完全停止前的修正次數及機構穩定性，需視實際轉動情況，經過不斷實驗後調整出一個適當的值。表3. 9 旗標TIMES、LorR、WINDOW副程式開始先讀取經過A/D Converter轉換後的值，並依照預先定義的轉換倍率做左旋或右旋(左旋一次代表乘以2、

30、右旋一次代表除以2)得到一組控制目標INPUT(INPUT0、INPUT1)。將INPUT的值與WINDOW相加即得到HIGH(HIGH0、HIGH1)，相減即得到LOW(LOW0、LOW1)。HIGH與LOW之間構成一個控制目標區間，此即為副程式最終目的。圖3. 8 副程式INPUT流程圖表3. 10 副程式INPUT程式碼3-4-4 副程式COMPARE 副程式DECODER計算編碼器解碼後的計數值(COUNTER)，代表目前機構(馬達)旋轉位置資訊。而副程式INPUT計算出一組控制目標區間(HIGH、LOW)，用以描述經由控制面板輸入的指令，代表希望機構(馬達)旋轉後於此區間中停下。 C

31、OMPARE的目的是比較上述兩種數值的大小，賦予POSITION旗標一個適當的數值。POSITION數值只有三種，用以描述目前機構位置與目標區間的關係，其意涵如表3.11所示。表3. 11 旗標POSITIONHIGH、LOW與COUNTER的儲存空間有兩個暫存器，因此必須先比較高位元暫存器再比較低位元暫存器，其流程如圖3.9。程式進行大小比較通常將兩個數值相減後，再依據狀態暫存器(R3，STATUS)的零旗標(Z)與借位旗標(C)判斷兩者的相互關係。當Z=1則減法運算結果為零，當C=1則減法運算結果為負數。圖3. 9 副程式COMPARE流程圖表3. 12 副程式COMPARE程式碼3-4-

32、5 副程式COMMAND 本區塊依據旗標POSITION、STATIC與SOM的值決定馬達控制指令。如3-6節所述，SOM具有五種狀態分別為靜止(S)、正轉(F)、慣性正轉(FS)、反轉(R)與慣性反轉(RS)，五種狀態間的轉換遵循一定流程如圖3.10。每個狀態皆有三種路線選擇，如表3.13，以POSITION與STATIC的值作為判斷標準，並以STATIC為優先判斷式。圖3. 10 旗標SOM轉換流程表3. 13 路線選擇判斷式 程式共分為15條(AO)路線，不同路線代表不同的馬達控制指令與旗標設定，詳細內容如表3.14。其中路線M、N是一種保護機制，避免機構運轉過程因不明原因卡住不動情況下

33、馬達持續運轉造成過熱燒毀，因此即刻停止馬達並將流程回到程式啟始點，手動校正機構位置並排除問題。表3. 14 各路線控制指令表3. 15 副程式COMMAND程式碼3-5 主程式及參數設定 程式最初需進行暫存器定義，作用相當於變數宣告，讓程式能夠進行運算、判斷式及儲存資料。其次是定義常用的工作暫存器名稱，增加程式可讀性。表3. 16 暫存器宣告 除了暫存器定義外，系統用到的輸出入腳位、常用位元旗標則以常數定義的方式宣告以增加程式可讀性，說明於表3.17中。表3. 17 常數宣告 參考3-7節的說明，因應各轉軸不同機構設計需預先定義適當之參數如表3.18及針對本次實驗之五軸機械臂每一轉軸的參數大小

34、如表3.19。表3. 18 參數定義表3. 19 五軸機械臂參數大小 這四項參數值設定依據各轉軸的馬達轉速、減速齒輪比及旋轉編碼器的特性及機構實際運作情形而定。其中adset1及adset2是經由上述組件的數值及機構轉動範圍運算而來，adset3及MAX則須依據實際運作情形調整適當之數值。而完整軔體程式也只需改變這四項參數值即可用於不同的轉軸。經過暫存器與常數宣告後，主程式開始進行初始設定，目的是宣告各輸出入埠形式、A/D轉換功能設定及賦予暫存器初始值。完整主程式詳述於表3.20，只要於主程式後加上本章節介紹的五個副程式模組，即可完成全部軔體系統。表3. 20 主程式第四章、高階語言模組化韌體

35、程式 以組合語言設計伺服控制韌體程式，其編碼過程可以充分利用處理器的獨特指令以及各種專門的硬體資源。然而必須解決耗時的機器級問題，如暫存器分配和指令排程，程式長且繁瑣，對於複雜之系統難以進行修改及維護。高階語言(C語言)則具有撰寫簡短與除錯容易之優勢，並具有高度可攜性容易移植於不同之平台。本章將第三章之組合語言改寫成高階語言之系統，並利用高階語言之優勢將伺服控制系統進一部改良，並達到模組化的目標。4-1 系統規劃ATMEL AT89C51微處理器是非常普遍之8051晶片，並能以KEIL編譯軟體進行C語言之程式設計。本章使用AT89C51微處理器，並擷取前面章節之開發經驗，重新建立伺服控制系統電

36、路架構(圖4.1)。以C語言開發具有高度可攜性的模組化韌體程式，修改原本一組微處理系統只能控制一組轉動機構的一對一模式，成為能有效節省硬體資源的一對多模式。電路架構主要分為三個區塊，第一區為指令傳遞模組負責人機介面及控制指令解讀與傳送，類似於第二章之控制面板及A/D轉換模組功能，此區塊主要作用為接收及傳送命令，與伺服控制核心較無直接相關，可彈性選用不同模組故不再贅述。第二區為實際進行伺服控制程式運算的核心，包含接收控制指令、回授信號解碼及改變驅動控制信號，此區使用一組AT89C51即可控制四組轉動機構，本章即著重於此區之伺服控制韌體程式開發。第三區則使用第二章所開發之泛用型驅動電路板，提供足夠

37、之驅動電流以控制各軸直流馬達。圖4.1 電路架構4-2 韌體程式架構韌體程式是參考第三章的內容進行開發，本章主要以C語言改寫，因此只針對程式流程做說明，詳細的程式控制觀念不再贅述。與第三章最大的差別在於原來只控制一組馬達，擴展到同時對四組馬達的伺服控制。程式中變數皆以陣列方式呈現，每個陣列有四個元素代表此晶片控制的四個軸編號，程式每個流程動作皆以迴圈方式對四組變數進行運算。如圖4.2，主要之迴圈包含讀取回授訊號、計算馬達位置、設定目標上下限、比較目標與位置差異、決定各軸下一步動作及輸出各軸命令。而第一區指令傳遞模組傳來之各軸目標命令則採用外部中斷之方式，其優點為能夠即時修改目標命令，無需預先設

38、定接收之時機，且能讓指令傳遞模組設計更為彈性。圖4.2 韌體架構 4-2-1 中斷函數 中斷函數(void command_int(void) interrupt 2)接收由指令傳遞模組發送之轉動目標命令，以AT89C51之P3_3(INT1)外部中斷腳位上緣觸發方式開啟中斷函數。當接收到中斷訊號時，暫停主程式流程進入中斷函數，讀取目標命令並重新設定變數anglei，此變數代表晶片控制之第i軸轉動目標距離原點角度及方位。anglei有效值為整數180至540，原點初始值為360，則各軸轉動範圍可達前後各180度。 目標命令包含三組變數(角度、方位、軸數)，由AT89C51輸出入埠接收信號(表4

39、.1)。角度有效值為0180，代表距離原點(初始位置)最大角度為180度。方位單位元變數以0與1表示原點逆向或順向。軸數有效值為015，最多可容納十六組馬達編號。表4. 1 目標命令 雖然每個AT89C51只能控制四組馬達，但可利用並聯的方式增加AT89C51以控制更多組馬達，故軸數命令容納十六組馬達編號，只需於中斷函數中區別此晶片控制哪些編號。圖4.3 中斷函數 4-2-2 decoder函數與旋轉編碼器回授信號有關的變數是eni與preni，分別表示馬達端第i軸編碼器目前準位與前次準位。進入函數前必須先讀取回授信號，由輸入埠P0_0P0_3接收四組旋轉編碼器回授信號，並將之分別寫入目前準位

40、變數(eni)。解碼函數(void decoder (unsigned char j)的運作原理，是藉由比較前後回授訊號是否改變，再依據馬達轉動狀態旗標(SOM)求出各轉軸目前位置資訊(sumi)。詳細流程可參考第三章3-4-2節的內容。圖4.4 decoder函數4-2-3 target函數中斷函數接收目標命令並轉成目標角度(anglei)，經由target函數求出一組具有上下限的轉動目標區間。運作原理是先將每一軸目標角度(anglei)換算成與位置資訊(sumi)相對應之計數值，因anglei之單位為角度，sumi之單位為編碼器脈波數，其換算方式因不同減數齒輪比及編碼器型號而異(參考3-4

41、-3節)。其次以窗型控制原理設定上下限(winHighi、winLowi)，容許轉動目標的誤差值，避免機構無法達到目標而異常作動，上下限的大小則依據實際運作結果做適當的調整。圖4.5 targer函數 4-2-4 compare函數比較函數(unsigned char compare(unsigned char j)決定位置變數(positioni)的內容，描述每一軸的位置是否已經進入轉動目標的窗型上下限。positioni有三種狀態，HIGH代表高於目標區間、LOW代表低於目標區間，而INSIDE則表示轉軸已轉動至目標區間內。圖4.6 compare函數4-2-5 decide與action

42、函數decide函數透過positioni與somi內容決定系統須對每一軸馬達下達哪一項命令，參考3-4-5節SOM轉換流程及路線選擇判斷依據，將此運作模式改寫成函數(void decide(unsigned char j)。decide函數輸出命令變數(commandi)，其內容為FORWARD、REVERSE及STOP三者之，再經由行動函數(void action(unsigned char j)解讀成最終之命令輸出至P1埠以控制馬達的轉動。圖4.7 decide與action函數第五章、兩相編碼器解碼電路設計 旋轉編碼器於直流馬達伺服控制中扮演重要角色，藉由旋轉編碼器之回授訊號才能做到閉

43、迴路控制的目標。之前章節開發過程受限於微處理機本身條件，在訊號解碼過程中只能採用單相解碼，不論在解析度或方向性上都遠不及兩相回授訊號。本章以數位邏輯電路方式實現兩相編碼器訊號解碼，不再受限於控制晶片的軔體功能或速度限制，並增加計數器IC的應用，以符合不同條件下的伺服控制系統。5-1 乘四解碼電路推導 如3-2節介紹，編碼器回授訊號主要原理是以霍爾磁感應電晶體產生之高低準位訊號波形，兩相編碼器A、B兩相電晶體在排列上差了90度相位差，因此其波形亦有90度的相位差，藉由相位先後順序得知轉動之方向，而判斷訊號高低準位改變次數即可得知轉動之角度。圖5.2兩相編碼器輸出波形訊號週期時間長短取決於旋轉速度

44、及編碼器每圈產生之週期數量，實際應用約為數微秒(S)至數毫秒(mS)之間。以第三章五軸機械臂為例，最短週期時間為1.1mS。而以麥柯昇公司之減速齒輪直流馬達組(maxon motor 345022)為例，其內建編碼器訊號經實際測試之最小週期時間只有12.75S(圖5.2)，如此短暫的週期已無法用前面章節之軔體系統進行解碼，故只能尋求硬體電路之方式。圖5.3 兩相編碼器訊號週期 電路中通常以微分的方式擷取原始訊號準位之改變，對訊號做微分就是取出訊號的斜率。理論上方波的左右邊緣斜率為無窮大，但真實世界的方波其上昇緣及下降緣放大來看還是有一定的上昇下降時間，也就是說其輸出波形為有限波峰大小的脈衝訊號

45、。實際應用於解碼電路中則無須真正求出波峰的大小，亦無須區分斜率的正負值，只需求得一組脈衝訊號能真實的反應每次訊號的改變。在硬體電路設計上，可先用D型正反器產生一個延遲訊號，與原訊號經過邏輯運算後，即可得到所需之脈衝訊號。 D型正反器應用於延遲訊號電路則需外加CLOCK訊號，其頻率須大於輸入訊號兩倍以上，否則輸入訊號將被濾除。依此特性，採用D型正反器延遲訊號可同時具有低通濾波之效果。實際電路輸入CLOCK頻率為4MHz，故頻率高於2MHz之雜訊將被濾除，留下的是週期大於0.5S之正常編碼器訊號。圖5.4 微分脈衝訊號 觀察圖5.3，脈衝訊號(PUA)於原始訊號(PHA)每次改變準位時產生，表5.

46、1為其邏輯真值表，此電路恰可用一互斥邏輯閘(XOR)產生。表5.1 脈衝訊號真值表 兩相脈衝訊號如圖5.4，因A、B兩相原始訊號有90度相位差，故其脈衝訊號剛好依序排列不會同時出現。以邏輯或閘(OR)將兩組脈衝訊號組合，則得到兩相完整脈衝訊號。個原始週期可得到四個脈衝訊號，此方法可提高原始訊號之解析度讓定位更為精準，稱為乘四解碼電路。以OMRON增量型旋轉編碼器E6A2-CW3E為例，編碼器旋轉一圈會產生200週期訊號，經過解碼後得到800個脈衝訊號，故計算脈衝訊號數量即可得知旋轉的角度。圖5.5 兩相脈衝訊號 脈衝訊號仍無法分辨轉動方向，可藉由D型正反器之特性產生一組轉動方向之參考訊號，如圖5.5。PHA為輸入訊號，PHB為D型正反器的CLK訊號，當PHB發生上緣觸發事件時PHA訊號準位會傳送至輸出Q。參考圖5.1，正轉輸出Q為高準位，反轉輸出Q為低準位，即可以此分辨轉動之方向。圖5.6 方向訊號5-2 解碼電路實測 本節依據5-1的推導過程製作實體電路板，電路如圖5.6所示，可同時對兩組編碼器解碼。圖5.7 解碼電路 以麥柯昇公司之減速齒輪直流馬達組(maxon motor 345022)做測試，第一步量測其編碼器訊號(PHA)與延遲訊號(DLA)，如圖5.7，兩者之間延遲時間約為0.2S。圖5.8 PHA與DLA