6 自由度機械手臂系統之建置與實作探討

Model and Treat for 6-DOF Robot Manipulators

研究生陳政安 撰

指導教授李青旻 博士

義守大學 電機工程學系碩士班

碩士論文

A Thesis Submitted to the Department of Electrical Engineering

of I-Shou University in Partial Fulfillment of the Requirements

for the Master Degree with a

Major in Electrical Engineering June 2008

Kaohsiung Taiwan Republic of China

中華民國 九十七 年 六 月

i

誌謝

在短暫的兩年研究生的生涯中指導教授李青旻博士不僅在論文研究的相關學識

與論文寫作上傾囊相授並且細心的教導也教育我正確的學術道德以及不吝嗇的

供給學生學術研究所需的資源讓學生無後顧之憂實為感謝隨著碩士生涯的結

束這些理念也引領著我航向往後的人生旅途同時也感謝口試委員台灣大學

電機系馮蟻剛教授義守大學電機系莊景文教授陳基發教授及工業技術研究院李

政翰博士針對論文與報告內容提供寶貴的建議與指正使得本論文得以更佳完

善充實

在此也要感謝研究室的各位同儕們偉盛威仁哲弘宜靜元恩以及學弟

們盈佐毓寧威廷致遠在這段時間不斷的給于各方面的協助讓本人能全

力用心地運用在論文的研究上

最後更要感謝父母在生活上與精神上的支持以及姐姐用心地給予學術上的幫

忙感謝你們

ii

6 自由度機械手臂系統之建置與實作探討

研究生陳政安 指導教授李青旻 博士

義守大學電機工程研究所

摘要

本論文主要討論具六自由度之機械手臂的系統模型的定位控制及實作具六自

由度之機器手臂是常見的軌跡可控制系統因機械手臂的結構與本身的材質及重量

甚至是外在環境的干擾及系統本身條件的限制等都會影響手臂移動的速度與抓取

物體的準確度因此我們需要設計適合的控制器控制機械手臂以便能定位於目

標位置在可承載物體重量的情況下我們也將調整機械手臂使其能有效地定位於

目標位置所討論之實體機械手臂的結構為使用伺服馬達的齒輪來連結以減少

連桿間相互作用的動態影響控制器的設計將使用 PD 控制器來做機械手臂的定位

控制並將實體的六自由度系統應用於機械蟲的定位控制

關鍵字 六自由度PD 控制器機械手臂伺服馬達定位控制

iii

Model and Treat for 6-DOF Robot Manipulators

Student Cheng-An Chen Advisor Dr Ching-Min Lee

Department of Electrical Engineering

I-Shou University

ABSTRACT

In the thesis it mainly considers position control problems for robot manipulator

systems with 6-DOF which are usual controllable trace systems Since the speed and

accuracy of the motions for a robot manipulator in position control problems are affected

not only by the structure weight and materials of the system but also by the disturbance

of the external environment and constraints for the robot system it is required that

designing a suitable controller to maintain its accuracy in position control problems One

PD controller is introduced On the other hand to decrease the dynamic interplay of the

connected rods for the robot system the practical robot manipulators are linked with the

wheelworks of DC servo motors The 6-DOF robot system will also be applied to

implement a robot insect motion

Keywordssix-degree of freedom PD controller robot manipulator DC servo position

control

iv

目錄 誌謝helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipi 中文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip ii 英文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiii 目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiv 圖目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipvi 表目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipvii

第一章 緒論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1

11 前言helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1 12 研究動機與方法helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1 13 論文架構helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip2

第二章 機械手臂控制理論介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4 21 機械手臂系統模型helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4

211 動態方程式helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4 212 手臂系統模型helliphelliphelliphelliphellip helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5

22 PD控制器 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 7 221 機械手臂定位控制helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip7

第三章 硬體架構說明helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9

31 硬體架構helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 32 機械手臂helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9

321 N 自由度介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 10 322 外觀模型建立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 323 伺服馬達介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 324 LPT 通訊協定介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19

第四章 軟體規劃 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 23

41 軟體架構說明helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 42 Matlab 環境介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 24

421 Simulink 工作環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 25 422 S-Function 介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 423 運用 Simulink 設計 PD 控制器helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip31

424 實驗結果 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 33 43 Visual Studio 2003 環境介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip37 431 程式軟體架構流程helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42

v

432 程式執行頁面讀取流程helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 43 44 實驗結果與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44

441 機械手臂helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 442 機械蟲helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 443 問題與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

第五章 結論 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46

51 討論分析helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46 52 未來研究方向helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47 53 研究心得helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47

參考文獻helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip49

附錄 A 程式碼 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip52

vi

圖目錄

2-1 機械手臂示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5 3-1 硬體架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-2 零件明細helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-3 機械手臂組裝機構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 3-4 機械手臂基底組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-5 伺服馬達組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-6 機械手臂組合完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-7 伺服馬達與驅動器整線完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-8 伺服馬達與驅動器連結helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-9 伺服馬達內部結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-10 伺服馬達閉迴路回饋系統helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip16 3-11 伺服馬達接線示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-12 HSS-422 伺服馬達外觀helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-13 並列埠模式設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip20 3-14 LPT 各接點串流位址對應helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip21 3-15 LPT 定址設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip22 4-1 系統架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 4-2 MATLAB 結構關係圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip25 4-3 功能方塊總集helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-4 進入 Simulink 按鈕helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-5 進入mdl 設計區helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 4-6 控制器設計連結圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip33 4-7 Q1 關節角度定位誤差 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 34 4-8 Q2 關節角度定位誤差helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip35 4-9 Q1 關節角度定位誤差（m1=0185kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-10 Q2 關節角度定位誤差（me=0122kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件hellip38 4-12 NET 允許不同程式語言於使用受限的環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip39 4-13 於「啟始頁」中設定偏好設定值helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-14 dll 檔轉換設定示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-15 軟體架構示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42 4-16 軟體控制介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip43 4-17 機械手臂外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 4-18 機械蟲外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

vii

表目錄 2-1 機械手臂各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip8 3-1 脈波寬度與角度關係表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-2 伺服馬達規格helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-3 伺服馬達步階規格表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 4-1 S-Function 各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip29

1

第一章 緒論

11 前言

近年來半導體業者不斷的致力於無人操控機台及機台人員操控設備的自動化

在無人操控的狀態下機械手臂的操控必需相對提升其穩定度及靈活性才能準確

的完成電子電路零組件的鋪線 (如自動焊接機台點焊搬運機器手臂多用途搬

運機器人焊接切割機器人)在高精密度需求的半導體製程公司手臂控制系統必

須具有高度的準確性(如使用機器手臂將 IC 零件抓取放置於自動焊接機台的 PCB

模組的正確位置讓 PCB 板上完成點焊的動作)而在具有高度危險的工作環境中(如

高幅射的機台設備及高放射性醫療儀器等)也須藉助機器手臂來完成特定任務

所以機器手臂的操控與研究變得非常重要對需要手動操控的機台設備操控者也

能更方便的使用此項設備由此可知機器手臂應用範圍很廣泛由於精確性及穩

定性的要求如何降低系統內部與外部的不確定性及干擾對系統的影響因此對

於機械手臂之應用系統也就變得很重要

12 研究動機與方法

在業界中伺服馬達常使用 PI 控制器來調整直流無刷馬達的電流控制迴路

並需自行調整 PI 控制器參數使得電流迴路擁有符合實際動作的性能需求但本

研究考慮到（1）物理的運動方程式與運動產生的慣量（2）因牛頓運動第二定律 F

= ma 手臂擺動受到加速度的影響易產生的不穩定情況（3）機械手臂向下擺動

的向心力因此建立機械手臂所需的 PD 控制器在角度定位控制則是以三維

空間做為機械手臂所能延展的運動方向及考慮到機械手臂上的伺服馬達由下至

上的擺動所產生的加速度最後實體系統所連結的6顆伺服馬達將由控制器的設

計並透過Simulink及Visual Studio 2003 軟體上的設計來取得六顆伺服馬達的平衡

點在軟體上所設定的參數則尚需微調手臂上的零件與軟體上所設定的角度定位

參數吻合直至能準確定位於目標位置

2

利用 PD 控制器來控制機械手臂優點為可增大阻尼比提高系統穩定度使

用其來直接定位缺點是如果受到外在環境的影響如受到負載增加受外力干擾

等因素所定位的角度則會產生些微偏差而不能自我調整如果改良伺服馬達的

構造於伺服馬達內部的前端加裝熱感應器採用 PID 控制器來設計定位控制可

使系統增加強健性的功能受到外力的影響時感應器接收到感應定位的位置有些

微偏差而能自動調整角度至定位

在本論文中我們將討論機械手臂在具有 6 個自由度的方向的控制利用向量

轉動的方式分析其穩定性並採用線性系統的解析方式及 Lyapunov 穩定方法來討論

此系統[8][10]吾人將採用的物理向量及控制系統模型[13][22][23]根據手臂的運

動方式討論其動態方程式將手臂模型線性化可參考文獻[4][5]而透過手臂本身具

有向下引力等物理現象來推導出公式並驗證

13 論文架構

第一章為本論文的簡介以簡短的方式讓讀者了解到機械手臂的初步概念及應

用技術面的探討由第二章開始考慮到如何將它實現並考慮成本上的問題

採用 DC 伺服馬達來做控制控制器的設計是使用 PD 控制器來將它實現做為定位

控制並且介紹所使用的數學模型的推算及驗證

第三章的部份則是如何實現將 DC 伺服馬達應用於連桿裝置做為機械手臂的

前臂使用伺服馬達的齒輪的互相連結可以達成可彎曲的動作並且由以下章節

逐一介紹機台的建立與所需要的工作環境及傳輸的模式

第四章的部份則介紹在軟體上的所需要做的規劃如何將硬體的部份透過傳

輸的方式編寫語言程式建立軟體並控制硬體透過軟體規劃流程表做為此系統軟

體的架構使軟體與硬體能於同步進行做控制並能讀取即時的數據做變更能馬上

讀取數據資料而硬體的部份能即時做轉換整個實驗過程結果做一整理並配合

3

圖片數據說明本論文的實際動作提供往後的相關人員作為可用的經驗與數據

最後一章將討論分析本論文的實驗過程研究心得以及未來方向並以簡短的

方式紀錄這一年半的實驗心得與感想緊接著帶出專題往後的發展與後續可研究的

方向並以商業行為的角度試著提出往後可行性的穩健的技術為發展方向作為

本論文的結尾

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

i

誌謝

在短暫的兩年研究生的生涯中指導教授李青旻博士不僅在論文研究的相關學識

與論文寫作上傾囊相授並且細心的教導也教育我正確的學術道德以及不吝嗇的

供給學生學術研究所需的資源讓學生無後顧之憂實為感謝隨著碩士生涯的結

束這些理念也引領著我航向往後的人生旅途同時也感謝口試委員台灣大學

電機系馮蟻剛教授義守大學電機系莊景文教授陳基發教授及工業技術研究院李

政翰博士針對論文與報告內容提供寶貴的建議與指正使得本論文得以更佳完

善充實

在此也要感謝研究室的各位同儕們偉盛威仁哲弘宜靜元恩以及學弟

們盈佐毓寧威廷致遠在這段時間不斷的給于各方面的協助讓本人能全

力用心地運用在論文的研究上

最後更要感謝父母在生活上與精神上的支持以及姐姐用心地給予學術上的幫

忙感謝你們

ii

6 自由度機械手臂系統之建置與實作探討

研究生陳政安 指導教授李青旻 博士

義守大學電機工程研究所

摘要

本論文主要討論具六自由度之機械手臂的系統模型的定位控制及實作具六自

由度之機器手臂是常見的軌跡可控制系統因機械手臂的結構與本身的材質及重量

甚至是外在環境的干擾及系統本身條件的限制等都會影響手臂移動的速度與抓取

物體的準確度因此我們需要設計適合的控制器控制機械手臂以便能定位於目

標位置在可承載物體重量的情況下我們也將調整機械手臂使其能有效地定位於

目標位置所討論之實體機械手臂的結構為使用伺服馬達的齒輪來連結以減少

連桿間相互作用的動態影響控制器的設計將使用 PD 控制器來做機械手臂的定位

控制並將實體的六自由度系統應用於機械蟲的定位控制

關鍵字 六自由度PD 控制器機械手臂伺服馬達定位控制

iii

Model and Treat for 6-DOF Robot Manipulators

Student Cheng-An Chen Advisor Dr Ching-Min Lee

Department of Electrical Engineering

I-Shou University

ABSTRACT

In the thesis it mainly considers position control problems for robot manipulator

systems with 6-DOF which are usual controllable trace systems Since the speed and

accuracy of the motions for a robot manipulator in position control problems are affected

not only by the structure weight and materials of the system but also by the disturbance

of the external environment and constraints for the robot system it is required that

designing a suitable controller to maintain its accuracy in position control problems One

PD controller is introduced On the other hand to decrease the dynamic interplay of the

connected rods for the robot system the practical robot manipulators are linked with the

wheelworks of DC servo motors The 6-DOF robot system will also be applied to

implement a robot insect motion

Keywordssix-degree of freedom PD controller robot manipulator DC servo position

control

iv

目錄 誌謝helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipi 中文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip ii 英文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiii 目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiv 圖目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipvi 表目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipvii

第一章 緒論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1

11 前言helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1 12 研究動機與方法helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1 13 論文架構helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip2

第二章 機械手臂控制理論介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4 21 機械手臂系統模型helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4

211 動態方程式helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4 212 手臂系統模型helliphelliphelliphelliphellip helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5

22 PD控制器 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 7 221 機械手臂定位控制helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip7

第三章 硬體架構說明helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9

31 硬體架構helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 32 機械手臂helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9

321 N 自由度介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 10 322 外觀模型建立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 323 伺服馬達介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 324 LPT 通訊協定介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19

第四章 軟體規劃 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 23

41 軟體架構說明helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 42 Matlab 環境介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 24

421 Simulink 工作環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 25 422 S-Function 介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 423 運用 Simulink 設計 PD 控制器helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip31

424 實驗結果 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 33 43 Visual Studio 2003 環境介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip37 431 程式軟體架構流程helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42

v

432 程式執行頁面讀取流程helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 43 44 實驗結果與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44

441 機械手臂helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 442 機械蟲helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 443 問題與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

第五章 結論 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46

51 討論分析helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46 52 未來研究方向helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47 53 研究心得helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47

參考文獻helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip49

附錄 A 程式碼 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip52

vi

圖目錄

2-1 機械手臂示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5 3-1 硬體架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-2 零件明細helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-3 機械手臂組裝機構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 3-4 機械手臂基底組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-5 伺服馬達組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-6 機械手臂組合完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-7 伺服馬達與驅動器整線完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-8 伺服馬達與驅動器連結helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-9 伺服馬達內部結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-10 伺服馬達閉迴路回饋系統helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip16 3-11 伺服馬達接線示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-12 HSS-422 伺服馬達外觀helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-13 並列埠模式設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip20 3-14 LPT 各接點串流位址對應helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip21 3-15 LPT 定址設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip22 4-1 系統架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 4-2 MATLAB 結構關係圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip25 4-3 功能方塊總集helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-4 進入 Simulink 按鈕helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-5 進入mdl 設計區helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 4-6 控制器設計連結圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip33 4-7 Q1 關節角度定位誤差 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 34 4-8 Q2 關節角度定位誤差helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip35 4-9 Q1 關節角度定位誤差（m1=0185kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-10 Q2 關節角度定位誤差（me=0122kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件hellip38 4-12 NET 允許不同程式語言於使用受限的環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip39 4-13 於「啟始頁」中設定偏好設定值helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-14 dll 檔轉換設定示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-15 軟體架構示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42 4-16 軟體控制介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip43 4-17 機械手臂外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 4-18 機械蟲外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

vii

表目錄 2-1 機械手臂各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip8 3-1 脈波寬度與角度關係表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-2 伺服馬達規格helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-3 伺服馬達步階規格表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 4-1 S-Function 各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip29

1

第一章 緒論

11 前言

近年來半導體業者不斷的致力於無人操控機台及機台人員操控設備的自動化

在無人操控的狀態下機械手臂的操控必需相對提升其穩定度及靈活性才能準確

的完成電子電路零組件的鋪線 (如自動焊接機台點焊搬運機器手臂多用途搬

運機器人焊接切割機器人)在高精密度需求的半導體製程公司手臂控制系統必

須具有高度的準確性(如使用機器手臂將 IC 零件抓取放置於自動焊接機台的 PCB

模組的正確位置讓 PCB 板上完成點焊的動作)而在具有高度危險的工作環境中(如

高幅射的機台設備及高放射性醫療儀器等)也須藉助機器手臂來完成特定任務

所以機器手臂的操控與研究變得非常重要對需要手動操控的機台設備操控者也

能更方便的使用此項設備由此可知機器手臂應用範圍很廣泛由於精確性及穩

定性的要求如何降低系統內部與外部的不確定性及干擾對系統的影響因此對

於機械手臂之應用系統也就變得很重要

12 研究動機與方法

在業界中伺服馬達常使用 PI 控制器來調整直流無刷馬達的電流控制迴路

並需自行調整 PI 控制器參數使得電流迴路擁有符合實際動作的性能需求但本

研究考慮到（1）物理的運動方程式與運動產生的慣量（2）因牛頓運動第二定律 F

= ma 手臂擺動受到加速度的影響易產生的不穩定情況（3）機械手臂向下擺動

的向心力因此建立機械手臂所需的 PD 控制器在角度定位控制則是以三維

空間做為機械手臂所能延展的運動方向及考慮到機械手臂上的伺服馬達由下至

上的擺動所產生的加速度最後實體系統所連結的6顆伺服馬達將由控制器的設

計並透過Simulink及Visual Studio 2003 軟體上的設計來取得六顆伺服馬達的平衡

點在軟體上所設定的參數則尚需微調手臂上的零件與軟體上所設定的角度定位

參數吻合直至能準確定位於目標位置

2

利用 PD 控制器來控制機械手臂優點為可增大阻尼比提高系統穩定度使

用其來直接定位缺點是如果受到外在環境的影響如受到負載增加受外力干擾

等因素所定位的角度則會產生些微偏差而不能自我調整如果改良伺服馬達的

構造於伺服馬達內部的前端加裝熱感應器採用 PID 控制器來設計定位控制可

使系統增加強健性的功能受到外力的影響時感應器接收到感應定位的位置有些

微偏差而能自動調整角度至定位

在本論文中我們將討論機械手臂在具有 6 個自由度的方向的控制利用向量

轉動的方式分析其穩定性並採用線性系統的解析方式及 Lyapunov 穩定方法來討論

此系統[8][10]吾人將採用的物理向量及控制系統模型[13][22][23]根據手臂的運

動方式討論其動態方程式將手臂模型線性化可參考文獻[4][5]而透過手臂本身具

有向下引力等物理現象來推導出公式並驗證

13 論文架構

第一章為本論文的簡介以簡短的方式讓讀者了解到機械手臂的初步概念及應

用技術面的探討由第二章開始考慮到如何將它實現並考慮成本上的問題

採用 DC 伺服馬達來做控制控制器的設計是使用 PD 控制器來將它實現做為定位

控制並且介紹所使用的數學模型的推算及驗證

第三章的部份則是如何實現將 DC 伺服馬達應用於連桿裝置做為機械手臂的

前臂使用伺服馬達的齒輪的互相連結可以達成可彎曲的動作並且由以下章節

逐一介紹機台的建立與所需要的工作環境及傳輸的模式

第四章的部份則介紹在軟體上的所需要做的規劃如何將硬體的部份透過傳

輸的方式編寫語言程式建立軟體並控制硬體透過軟體規劃流程表做為此系統軟

體的架構使軟體與硬體能於同步進行做控制並能讀取即時的數據做變更能馬上

讀取數據資料而硬體的部份能即時做轉換整個實驗過程結果做一整理並配合

3

圖片數據說明本論文的實際動作提供往後的相關人員作為可用的經驗與數據

最後一章將討論分析本論文的實驗過程研究心得以及未來方向並以簡短的

方式紀錄這一年半的實驗心得與感想緊接著帶出專題往後的發展與後續可研究的

方向並以商業行為的角度試著提出往後可行性的穩健的技術為發展方向作為

本論文的結尾

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

ii

6 自由度機械手臂系統之建置與實作探討

研究生陳政安 指導教授李青旻 博士

義守大學電機工程研究所

摘要

本論文主要討論具六自由度之機械手臂的系統模型的定位控制及實作具六自

由度之機器手臂是常見的軌跡可控制系統因機械手臂的結構與本身的材質及重量

甚至是外在環境的干擾及系統本身條件的限制等都會影響手臂移動的速度與抓取

物體的準確度因此我們需要設計適合的控制器控制機械手臂以便能定位於目

標位置在可承載物體重量的情況下我們也將調整機械手臂使其能有效地定位於

目標位置所討論之實體機械手臂的結構為使用伺服馬達的齒輪來連結以減少

連桿間相互作用的動態影響控制器的設計將使用 PD 控制器來做機械手臂的定位

控制並將實體的六自由度系統應用於機械蟲的定位控制

關鍵字 六自由度PD 控制器機械手臂伺服馬達定位控制

iii

Model and Treat for 6-DOF Robot Manipulators

Student Cheng-An Chen Advisor Dr Ching-Min Lee

Department of Electrical Engineering

I-Shou University

ABSTRACT

In the thesis it mainly considers position control problems for robot manipulator

systems with 6-DOF which are usual controllable trace systems Since the speed and

accuracy of the motions for a robot manipulator in position control problems are affected

not only by the structure weight and materials of the system but also by the disturbance

of the external environment and constraints for the robot system it is required that

designing a suitable controller to maintain its accuracy in position control problems One

PD controller is introduced On the other hand to decrease the dynamic interplay of the

connected rods for the robot system the practical robot manipulators are linked with the

wheelworks of DC servo motors The 6-DOF robot system will also be applied to

implement a robot insect motion

Keywordssix-degree of freedom PD controller robot manipulator DC servo position

control

iv

目錄 誌謝helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipi 中文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip ii 英文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiii 目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiv 圖目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipvi 表目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipvii

第一章 緒論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1

11 前言helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1 12 研究動機與方法helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1 13 論文架構helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip2

第二章 機械手臂控制理論介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4 21 機械手臂系統模型helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4

211 動態方程式helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4 212 手臂系統模型helliphelliphelliphelliphellip helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5

22 PD控制器 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 7 221 機械手臂定位控制helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip7

第三章 硬體架構說明helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9

31 硬體架構helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 32 機械手臂helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9

321 N 自由度介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 10 322 外觀模型建立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 323 伺服馬達介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 324 LPT 通訊協定介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19

第四章 軟體規劃 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 23

41 軟體架構說明helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 42 Matlab 環境介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 24

421 Simulink 工作環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 25 422 S-Function 介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 423 運用 Simulink 設計 PD 控制器helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip31

424 實驗結果 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 33 43 Visual Studio 2003 環境介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip37 431 程式軟體架構流程helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42

v

432 程式執行頁面讀取流程helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 43 44 實驗結果與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44

441 機械手臂helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 442 機械蟲helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 443 問題與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

第五章 結論 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46

51 討論分析helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46 52 未來研究方向helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47 53 研究心得helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47

參考文獻helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip49

附錄 A 程式碼 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip52

vi

圖目錄

2-1 機械手臂示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5 3-1 硬體架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-2 零件明細helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-3 機械手臂組裝機構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 3-4 機械手臂基底組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-5 伺服馬達組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-6 機械手臂組合完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-7 伺服馬達與驅動器整線完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-8 伺服馬達與驅動器連結helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-9 伺服馬達內部結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-10 伺服馬達閉迴路回饋系統helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip16 3-11 伺服馬達接線示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-12 HSS-422 伺服馬達外觀helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-13 並列埠模式設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip20 3-14 LPT 各接點串流位址對應helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip21 3-15 LPT 定址設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip22 4-1 系統架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 4-2 MATLAB 結構關係圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip25 4-3 功能方塊總集helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-4 進入 Simulink 按鈕helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-5 進入mdl 設計區helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 4-6 控制器設計連結圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip33 4-7 Q1 關節角度定位誤差 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 34 4-8 Q2 關節角度定位誤差helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip35 4-9 Q1 關節角度定位誤差（m1=0185kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-10 Q2 關節角度定位誤差（me=0122kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件hellip38 4-12 NET 允許不同程式語言於使用受限的環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip39 4-13 於「啟始頁」中設定偏好設定值helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-14 dll 檔轉換設定示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-15 軟體架構示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42 4-16 軟體控制介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip43 4-17 機械手臂外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 4-18 機械蟲外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

vii

表目錄 2-1 機械手臂各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip8 3-1 脈波寬度與角度關係表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-2 伺服馬達規格helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-3 伺服馬達步階規格表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 4-1 S-Function 各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip29

1

第一章 緒論

11 前言

近年來半導體業者不斷的致力於無人操控機台及機台人員操控設備的自動化

在無人操控的狀態下機械手臂的操控必需相對提升其穩定度及靈活性才能準確

的完成電子電路零組件的鋪線 (如自動焊接機台點焊搬運機器手臂多用途搬

運機器人焊接切割機器人)在高精密度需求的半導體製程公司手臂控制系統必

須具有高度的準確性(如使用機器手臂將 IC 零件抓取放置於自動焊接機台的 PCB

模組的正確位置讓 PCB 板上完成點焊的動作)而在具有高度危險的工作環境中(如

高幅射的機台設備及高放射性醫療儀器等)也須藉助機器手臂來完成特定任務

所以機器手臂的操控與研究變得非常重要對需要手動操控的機台設備操控者也

能更方便的使用此項設備由此可知機器手臂應用範圍很廣泛由於精確性及穩

定性的要求如何降低系統內部與外部的不確定性及干擾對系統的影響因此對

於機械手臂之應用系統也就變得很重要

12 研究動機與方法

在業界中伺服馬達常使用 PI 控制器來調整直流無刷馬達的電流控制迴路

並需自行調整 PI 控制器參數使得電流迴路擁有符合實際動作的性能需求但本

研究考慮到（1）物理的運動方程式與運動產生的慣量（2）因牛頓運動第二定律 F

= ma 手臂擺動受到加速度的影響易產生的不穩定情況（3）機械手臂向下擺動

的向心力因此建立機械手臂所需的 PD 控制器在角度定位控制則是以三維

空間做為機械手臂所能延展的運動方向及考慮到機械手臂上的伺服馬達由下至

上的擺動所產生的加速度最後實體系統所連結的6顆伺服馬達將由控制器的設

計並透過Simulink及Visual Studio 2003 軟體上的設計來取得六顆伺服馬達的平衡

點在軟體上所設定的參數則尚需微調手臂上的零件與軟體上所設定的角度定位

參數吻合直至能準確定位於目標位置

2

利用 PD 控制器來控制機械手臂優點為可增大阻尼比提高系統穩定度使

用其來直接定位缺點是如果受到外在環境的影響如受到負載增加受外力干擾

等因素所定位的角度則會產生些微偏差而不能自我調整如果改良伺服馬達的

構造於伺服馬達內部的前端加裝熱感應器採用 PID 控制器來設計定位控制可

使系統增加強健性的功能受到外力的影響時感應器接收到感應定位的位置有些

微偏差而能自動調整角度至定位

在本論文中我們將討論機械手臂在具有 6 個自由度的方向的控制利用向量

轉動的方式分析其穩定性並採用線性系統的解析方式及 Lyapunov 穩定方法來討論

此系統[8][10]吾人將採用的物理向量及控制系統模型[13][22][23]根據手臂的運

動方式討論其動態方程式將手臂模型線性化可參考文獻[4][5]而透過手臂本身具

有向下引力等物理現象來推導出公式並驗證

13 論文架構

第一章為本論文的簡介以簡短的方式讓讀者了解到機械手臂的初步概念及應

用技術面的探討由第二章開始考慮到如何將它實現並考慮成本上的問題

採用 DC 伺服馬達來做控制控制器的設計是使用 PD 控制器來將它實現做為定位

控制並且介紹所使用的數學模型的推算及驗證

第三章的部份則是如何實現將 DC 伺服馬達應用於連桿裝置做為機械手臂的

前臂使用伺服馬達的齒輪的互相連結可以達成可彎曲的動作並且由以下章節

逐一介紹機台的建立與所需要的工作環境及傳輸的模式

第四章的部份則介紹在軟體上的所需要做的規劃如何將硬體的部份透過傳

輸的方式編寫語言程式建立軟體並控制硬體透過軟體規劃流程表做為此系統軟

體的架構使軟體與硬體能於同步進行做控制並能讀取即時的數據做變更能馬上

讀取數據資料而硬體的部份能即時做轉換整個實驗過程結果做一整理並配合

3

圖片數據說明本論文的實際動作提供往後的相關人員作為可用的經驗與數據

最後一章將討論分析本論文的實驗過程研究心得以及未來方向並以簡短的

方式紀錄這一年半的實驗心得與感想緊接著帶出專題往後的發展與後續可研究的

方向並以商業行為的角度試著提出往後可行性的穩健的技術為發展方向作為

本論文的結尾

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

iii

Model and Treat for 6-DOF Robot Manipulators

Student Cheng-An Chen Advisor Dr Ching-Min Lee

Department of Electrical Engineering

I-Shou University

ABSTRACT

In the thesis it mainly considers position control problems for robot manipulator

systems with 6-DOF which are usual controllable trace systems Since the speed and

accuracy of the motions for a robot manipulator in position control problems are affected

not only by the structure weight and materials of the system but also by the disturbance

of the external environment and constraints for the robot system it is required that

designing a suitable controller to maintain its accuracy in position control problems One

PD controller is introduced On the other hand to decrease the dynamic interplay of the

connected rods for the robot system the practical robot manipulators are linked with the

wheelworks of DC servo motors The 6-DOF robot system will also be applied to

implement a robot insect motion

Keywordssix-degree of freedom PD controller robot manipulator DC servo position

control

iv

目錄 誌謝helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipi 中文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip ii 英文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiii 目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiv 圖目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipvi 表目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipvii

第一章 緒論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1

11 前言helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1 12 研究動機與方法helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1 13 論文架構helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip2

第二章 機械手臂控制理論介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4 21 機械手臂系統模型helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4

211 動態方程式helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4 212 手臂系統模型helliphelliphelliphelliphellip helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5

22 PD控制器 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 7 221 機械手臂定位控制helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip7

第三章 硬體架構說明helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9

31 硬體架構helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 32 機械手臂helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9

321 N 自由度介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 10 322 外觀模型建立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 323 伺服馬達介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 324 LPT 通訊協定介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19

第四章 軟體規劃 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 23

41 軟體架構說明helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 42 Matlab 環境介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 24

421 Simulink 工作環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 25 422 S-Function 介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 423 運用 Simulink 設計 PD 控制器helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip31

424 實驗結果 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 33 43 Visual Studio 2003 環境介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip37 431 程式軟體架構流程helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42

v

432 程式執行頁面讀取流程helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 43 44 實驗結果與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44

441 機械手臂helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 442 機械蟲helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 443 問題與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

第五章 結論 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46

51 討論分析helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46 52 未來研究方向helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47 53 研究心得helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47

參考文獻helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip49

附錄 A 程式碼 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip52

vi

圖目錄

2-1 機械手臂示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5 3-1 硬體架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-2 零件明細helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-3 機械手臂組裝機構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 3-4 機械手臂基底組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-5 伺服馬達組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-6 機械手臂組合完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-7 伺服馬達與驅動器整線完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-8 伺服馬達與驅動器連結helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-9 伺服馬達內部結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-10 伺服馬達閉迴路回饋系統helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip16 3-11 伺服馬達接線示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-12 HSS-422 伺服馬達外觀helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-13 並列埠模式設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip20 3-14 LPT 各接點串流位址對應helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip21 3-15 LPT 定址設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip22 4-1 系統架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 4-2 MATLAB 結構關係圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip25 4-3 功能方塊總集helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-4 進入 Simulink 按鈕helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-5 進入mdl 設計區helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 4-6 控制器設計連結圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip33 4-7 Q1 關節角度定位誤差 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 34 4-8 Q2 關節角度定位誤差helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip35 4-9 Q1 關節角度定位誤差（m1=0185kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-10 Q2 關節角度定位誤差（me=0122kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件hellip38 4-12 NET 允許不同程式語言於使用受限的環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip39 4-13 於「啟始頁」中設定偏好設定值helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-14 dll 檔轉換設定示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-15 軟體架構示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42 4-16 軟體控制介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip43 4-17 機械手臂外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 4-18 機械蟲外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

vii

表目錄 2-1 機械手臂各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip8 3-1 脈波寬度與角度關係表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-2 伺服馬達規格helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-3 伺服馬達步階規格表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 4-1 S-Function 各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip29

1

第一章 緒論

11 前言

近年來半導體業者不斷的致力於無人操控機台及機台人員操控設備的自動化

在無人操控的狀態下機械手臂的操控必需相對提升其穩定度及靈活性才能準確

的完成電子電路零組件的鋪線 (如自動焊接機台點焊搬運機器手臂多用途搬

運機器人焊接切割機器人)在高精密度需求的半導體製程公司手臂控制系統必

須具有高度的準確性(如使用機器手臂將 IC 零件抓取放置於自動焊接機台的 PCB

模組的正確位置讓 PCB 板上完成點焊的動作)而在具有高度危險的工作環境中(如

高幅射的機台設備及高放射性醫療儀器等)也須藉助機器手臂來完成特定任務

所以機器手臂的操控與研究變得非常重要對需要手動操控的機台設備操控者也

能更方便的使用此項設備由此可知機器手臂應用範圍很廣泛由於精確性及穩

定性的要求如何降低系統內部與外部的不確定性及干擾對系統的影響因此對

於機械手臂之應用系統也就變得很重要

12 研究動機與方法

在業界中伺服馬達常使用 PI 控制器來調整直流無刷馬達的電流控制迴路

並需自行調整 PI 控制器參數使得電流迴路擁有符合實際動作的性能需求但本

研究考慮到（1）物理的運動方程式與運動產生的慣量（2）因牛頓運動第二定律 F

= ma 手臂擺動受到加速度的影響易產生的不穩定情況（3）機械手臂向下擺動

的向心力因此建立機械手臂所需的 PD 控制器在角度定位控制則是以三維

空間做為機械手臂所能延展的運動方向及考慮到機械手臂上的伺服馬達由下至

上的擺動所產生的加速度最後實體系統所連結的6顆伺服馬達將由控制器的設

計並透過Simulink及Visual Studio 2003 軟體上的設計來取得六顆伺服馬達的平衡

點在軟體上所設定的參數則尚需微調手臂上的零件與軟體上所設定的角度定位

參數吻合直至能準確定位於目標位置

2

利用 PD 控制器來控制機械手臂優點為可增大阻尼比提高系統穩定度使

用其來直接定位缺點是如果受到外在環境的影響如受到負載增加受外力干擾

等因素所定位的角度則會產生些微偏差而不能自我調整如果改良伺服馬達的

構造於伺服馬達內部的前端加裝熱感應器採用 PID 控制器來設計定位控制可

使系統增加強健性的功能受到外力的影響時感應器接收到感應定位的位置有些

微偏差而能自動調整角度至定位

在本論文中我們將討論機械手臂在具有 6 個自由度的方向的控制利用向量

轉動的方式分析其穩定性並採用線性系統的解析方式及 Lyapunov 穩定方法來討論

此系統[8][10]吾人將採用的物理向量及控制系統模型[13][22][23]根據手臂的運

動方式討論其動態方程式將手臂模型線性化可參考文獻[4][5]而透過手臂本身具

有向下引力等物理現象來推導出公式並驗證

13 論文架構

第一章為本論文的簡介以簡短的方式讓讀者了解到機械手臂的初步概念及應

用技術面的探討由第二章開始考慮到如何將它實現並考慮成本上的問題

採用 DC 伺服馬達來做控制控制器的設計是使用 PD 控制器來將它實現做為定位

控制並且介紹所使用的數學模型的推算及驗證

第三章的部份則是如何實現將 DC 伺服馬達應用於連桿裝置做為機械手臂的

前臂使用伺服馬達的齒輪的互相連結可以達成可彎曲的動作並且由以下章節

逐一介紹機台的建立與所需要的工作環境及傳輸的模式

第四章的部份則介紹在軟體上的所需要做的規劃如何將硬體的部份透過傳

輸的方式編寫語言程式建立軟體並控制硬體透過軟體規劃流程表做為此系統軟

體的架構使軟體與硬體能於同步進行做控制並能讀取即時的數據做變更能馬上

讀取數據資料而硬體的部份能即時做轉換整個實驗過程結果做一整理並配合

3

圖片數據說明本論文的實際動作提供往後的相關人員作為可用的經驗與數據

最後一章將討論分析本論文的實驗過程研究心得以及未來方向並以簡短的

方式紀錄這一年半的實驗心得與感想緊接著帶出專題往後的發展與後續可研究的

方向並以商業行為的角度試著提出往後可行性的穩健的技術為發展方向作為

本論文的結尾

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

iv

目錄 誌謝helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipi 中文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip ii 英文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiii 目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiv 圖目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipvi 表目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipvii

第一章 緒論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1

11 前言helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1 12 研究動機與方法helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip1 13 論文架構helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip2

第二章 機械手臂控制理論介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4 21 機械手臂系統模型helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4

211 動態方程式helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip4 212 手臂系統模型helliphelliphelliphelliphellip helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5

22 PD控制器 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 7 221 機械手臂定位控制helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip7

第三章 硬體架構說明helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9

31 硬體架構helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 32 機械手臂helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9

321 N 自由度介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 10 322 外觀模型建立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 323 伺服馬達介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 324 LPT 通訊協定介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19

第四章 軟體規劃 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 23

41 軟體架構說明helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 42 Matlab 環境介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 24

421 Simulink 工作環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 25 422 S-Function 介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 423 運用 Simulink 設計 PD 控制器helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip31

424 實驗結果 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 33 43 Visual Studio 2003 環境介紹helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip37 431 程式軟體架構流程helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42

v

432 程式執行頁面讀取流程helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 43 44 實驗結果與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44

441 機械手臂helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 442 機械蟲helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 443 問題與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

第五章 結論 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46

51 討論分析helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46 52 未來研究方向helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47 53 研究心得helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47

參考文獻helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip49

附錄 A 程式碼 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip52

vi

圖目錄

2-1 機械手臂示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5 3-1 硬體架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-2 零件明細helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-3 機械手臂組裝機構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 3-4 機械手臂基底組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-5 伺服馬達組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-6 機械手臂組合完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-7 伺服馬達與驅動器整線完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-8 伺服馬達與驅動器連結helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-9 伺服馬達內部結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-10 伺服馬達閉迴路回饋系統helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip16 3-11 伺服馬達接線示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-12 HSS-422 伺服馬達外觀helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-13 並列埠模式設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip20 3-14 LPT 各接點串流位址對應helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip21 3-15 LPT 定址設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip22 4-1 系統架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 4-2 MATLAB 結構關係圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip25 4-3 功能方塊總集helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-4 進入 Simulink 按鈕helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-5 進入mdl 設計區helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 4-6 控制器設計連結圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip33 4-7 Q1 關節角度定位誤差 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 34 4-8 Q2 關節角度定位誤差helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip35 4-9 Q1 關節角度定位誤差（m1=0185kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-10 Q2 關節角度定位誤差（me=0122kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件hellip38 4-12 NET 允許不同程式語言於使用受限的環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip39 4-13 於「啟始頁」中設定偏好設定值helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-14 dll 檔轉換設定示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-15 軟體架構示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42 4-16 軟體控制介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip43 4-17 機械手臂外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 4-18 機械蟲外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

vii

表目錄 2-1 機械手臂各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip8 3-1 脈波寬度與角度關係表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-2 伺服馬達規格helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-3 伺服馬達步階規格表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 4-1 S-Function 各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip29

1

第一章 緒論

11 前言

近年來半導體業者不斷的致力於無人操控機台及機台人員操控設備的自動化

在無人操控的狀態下機械手臂的操控必需相對提升其穩定度及靈活性才能準確

的完成電子電路零組件的鋪線 (如自動焊接機台點焊搬運機器手臂多用途搬

運機器人焊接切割機器人)在高精密度需求的半導體製程公司手臂控制系統必

須具有高度的準確性(如使用機器手臂將 IC 零件抓取放置於自動焊接機台的 PCB

模組的正確位置讓 PCB 板上完成點焊的動作)而在具有高度危險的工作環境中(如

高幅射的機台設備及高放射性醫療儀器等)也須藉助機器手臂來完成特定任務

所以機器手臂的操控與研究變得非常重要對需要手動操控的機台設備操控者也

能更方便的使用此項設備由此可知機器手臂應用範圍很廣泛由於精確性及穩

定性的要求如何降低系統內部與外部的不確定性及干擾對系統的影響因此對

於機械手臂之應用系統也就變得很重要

12 研究動機與方法

在業界中伺服馬達常使用 PI 控制器來調整直流無刷馬達的電流控制迴路

並需自行調整 PI 控制器參數使得電流迴路擁有符合實際動作的性能需求但本

研究考慮到（1）物理的運動方程式與運動產生的慣量（2）因牛頓運動第二定律 F

= ma 手臂擺動受到加速度的影響易產生的不穩定情況（3）機械手臂向下擺動

的向心力因此建立機械手臂所需的 PD 控制器在角度定位控制則是以三維

空間做為機械手臂所能延展的運動方向及考慮到機械手臂上的伺服馬達由下至

上的擺動所產生的加速度最後實體系統所連結的6顆伺服馬達將由控制器的設

計並透過Simulink及Visual Studio 2003 軟體上的設計來取得六顆伺服馬達的平衡

點在軟體上所設定的參數則尚需微調手臂上的零件與軟體上所設定的角度定位

參數吻合直至能準確定位於目標位置

2

利用 PD 控制器來控制機械手臂優點為可增大阻尼比提高系統穩定度使

用其來直接定位缺點是如果受到外在環境的影響如受到負載增加受外力干擾

等因素所定位的角度則會產生些微偏差而不能自我調整如果改良伺服馬達的

構造於伺服馬達內部的前端加裝熱感應器採用 PID 控制器來設計定位控制可

使系統增加強健性的功能受到外力的影響時感應器接收到感應定位的位置有些

微偏差而能自動調整角度至定位

在本論文中我們將討論機械手臂在具有 6 個自由度的方向的控制利用向量

轉動的方式分析其穩定性並採用線性系統的解析方式及 Lyapunov 穩定方法來討論

此系統[8][10]吾人將採用的物理向量及控制系統模型[13][22][23]根據手臂的運

動方式討論其動態方程式將手臂模型線性化可參考文獻[4][5]而透過手臂本身具

有向下引力等物理現象來推導出公式並驗證

13 論文架構

第一章為本論文的簡介以簡短的方式讓讀者了解到機械手臂的初步概念及應

用技術面的探討由第二章開始考慮到如何將它實現並考慮成本上的問題

採用 DC 伺服馬達來做控制控制器的設計是使用 PD 控制器來將它實現做為定位

控制並且介紹所使用的數學模型的推算及驗證

第三章的部份則是如何實現將 DC 伺服馬達應用於連桿裝置做為機械手臂的

前臂使用伺服馬達的齒輪的互相連結可以達成可彎曲的動作並且由以下章節

逐一介紹機台的建立與所需要的工作環境及傳輸的模式

第四章的部份則介紹在軟體上的所需要做的規劃如何將硬體的部份透過傳

輸的方式編寫語言程式建立軟體並控制硬體透過軟體規劃流程表做為此系統軟

體的架構使軟體與硬體能於同步進行做控制並能讀取即時的數據做變更能馬上

讀取數據資料而硬體的部份能即時做轉換整個實驗過程結果做一整理並配合

3

圖片數據說明本論文的實際動作提供往後的相關人員作為可用的經驗與數據

最後一章將討論分析本論文的實驗過程研究心得以及未來方向並以簡短的

方式紀錄這一年半的實驗心得與感想緊接著帶出專題往後的發展與後續可研究的

方向並以商業行為的角度試著提出往後可行性的穩健的技術為發展方向作為

本論文的結尾

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

v

432 程式執行頁面讀取流程helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 43 44 實驗結果與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44

441 機械手臂helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 442 機械蟲helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 443 問題與討論helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

第五章 結論 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46

51 討論分析helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip46 52 未來研究方向helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47 53 研究心得helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip47

參考文獻helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip49

附錄 A 程式碼 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip52

vi

圖目錄

2-1 機械手臂示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5 3-1 硬體架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-2 零件明細helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-3 機械手臂組裝機構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 3-4 機械手臂基底組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-5 伺服馬達組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-6 機械手臂組合完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-7 伺服馬達與驅動器整線完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-8 伺服馬達與驅動器連結helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-9 伺服馬達內部結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-10 伺服馬達閉迴路回饋系統helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip16 3-11 伺服馬達接線示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-12 HSS-422 伺服馬達外觀helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-13 並列埠模式設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip20 3-14 LPT 各接點串流位址對應helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip21 3-15 LPT 定址設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip22 4-1 系統架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 4-2 MATLAB 結構關係圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip25 4-3 功能方塊總集helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-4 進入 Simulink 按鈕helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-5 進入mdl 設計區helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 4-6 控制器設計連結圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip33 4-7 Q1 關節角度定位誤差 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 34 4-8 Q2 關節角度定位誤差helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip35 4-9 Q1 關節角度定位誤差（m1=0185kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-10 Q2 關節角度定位誤差（me=0122kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件hellip38 4-12 NET 允許不同程式語言於使用受限的環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip39 4-13 於「啟始頁」中設定偏好設定值helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-14 dll 檔轉換設定示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-15 軟體架構示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42 4-16 軟體控制介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip43 4-17 機械手臂外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 4-18 機械蟲外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

vii

表目錄 2-1 機械手臂各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip8 3-1 脈波寬度與角度關係表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-2 伺服馬達規格helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-3 伺服馬達步階規格表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 4-1 S-Function 各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip29

1

第一章 緒論

11 前言

近年來半導體業者不斷的致力於無人操控機台及機台人員操控設備的自動化

在無人操控的狀態下機械手臂的操控必需相對提升其穩定度及靈活性才能準確

的完成電子電路零組件的鋪線 (如自動焊接機台點焊搬運機器手臂多用途搬

運機器人焊接切割機器人)在高精密度需求的半導體製程公司手臂控制系統必

須具有高度的準確性(如使用機器手臂將 IC 零件抓取放置於自動焊接機台的 PCB

模組的正確位置讓 PCB 板上完成點焊的動作)而在具有高度危險的工作環境中(如

高幅射的機台設備及高放射性醫療儀器等)也須藉助機器手臂來完成特定任務

所以機器手臂的操控與研究變得非常重要對需要手動操控的機台設備操控者也

能更方便的使用此項設備由此可知機器手臂應用範圍很廣泛由於精確性及穩

定性的要求如何降低系統內部與外部的不確定性及干擾對系統的影響因此對

於機械手臂之應用系統也就變得很重要

12 研究動機與方法

在業界中伺服馬達常使用 PI 控制器來調整直流無刷馬達的電流控制迴路

並需自行調整 PI 控制器參數使得電流迴路擁有符合實際動作的性能需求但本

研究考慮到（1）物理的運動方程式與運動產生的慣量（2）因牛頓運動第二定律 F

= ma 手臂擺動受到加速度的影響易產生的不穩定情況（3）機械手臂向下擺動

的向心力因此建立機械手臂所需的 PD 控制器在角度定位控制則是以三維

空間做為機械手臂所能延展的運動方向及考慮到機械手臂上的伺服馬達由下至

上的擺動所產生的加速度最後實體系統所連結的6顆伺服馬達將由控制器的設

計並透過Simulink及Visual Studio 2003 軟體上的設計來取得六顆伺服馬達的平衡

點在軟體上所設定的參數則尚需微調手臂上的零件與軟體上所設定的角度定位

參數吻合直至能準確定位於目標位置

2

利用 PD 控制器來控制機械手臂優點為可增大阻尼比提高系統穩定度使

用其來直接定位缺點是如果受到外在環境的影響如受到負載增加受外力干擾

等因素所定位的角度則會產生些微偏差而不能自我調整如果改良伺服馬達的

構造於伺服馬達內部的前端加裝熱感應器採用 PID 控制器來設計定位控制可

使系統增加強健性的功能受到外力的影響時感應器接收到感應定位的位置有些

微偏差而能自動調整角度至定位

在本論文中我們將討論機械手臂在具有 6 個自由度的方向的控制利用向量

轉動的方式分析其穩定性並採用線性系統的解析方式及 Lyapunov 穩定方法來討論

此系統[8][10]吾人將採用的物理向量及控制系統模型[13][22][23]根據手臂的運

動方式討論其動態方程式將手臂模型線性化可參考文獻[4][5]而透過手臂本身具

有向下引力等物理現象來推導出公式並驗證

13 論文架構

第一章為本論文的簡介以簡短的方式讓讀者了解到機械手臂的初步概念及應

用技術面的探討由第二章開始考慮到如何將它實現並考慮成本上的問題

採用 DC 伺服馬達來做控制控制器的設計是使用 PD 控制器來將它實現做為定位

控制並且介紹所使用的數學模型的推算及驗證

第三章的部份則是如何實現將 DC 伺服馬達應用於連桿裝置做為機械手臂的

前臂使用伺服馬達的齒輪的互相連結可以達成可彎曲的動作並且由以下章節

逐一介紹機台的建立與所需要的工作環境及傳輸的模式

第四章的部份則介紹在軟體上的所需要做的規劃如何將硬體的部份透過傳

輸的方式編寫語言程式建立軟體並控制硬體透過軟體規劃流程表做為此系統軟

體的架構使軟體與硬體能於同步進行做控制並能讀取即時的數據做變更能馬上

讀取數據資料而硬體的部份能即時做轉換整個實驗過程結果做一整理並配合

3

圖片數據說明本論文的實際動作提供往後的相關人員作為可用的經驗與數據

最後一章將討論分析本論文的實驗過程研究心得以及未來方向並以簡短的

方式紀錄這一年半的實驗心得與感想緊接著帶出專題往後的發展與後續可研究的

方向並以商業行為的角度試著提出往後可行性的穩健的技術為發展方向作為

本論文的結尾

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

vi

圖目錄

2-1 機械手臂示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip5 3-1 硬體架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-2 零件明細helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip9 3-3 機械手臂組裝機構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip12 3-4 機械手臂基底組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-5 伺服馬達組立helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip13 3-6 機械手臂組合完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-7 伺服馬達與驅動器整線完成圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip14 3-8 伺服馬達與驅動器連結helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-9 伺服馬達內部結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip15 3-10 伺服馬達閉迴路回饋系統helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip16 3-11 伺服馬達接線示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-12 HSS-422 伺服馬達外觀helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-13 並列埠模式設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip20 3-14 LPT 各接點串流位址對應helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip21 3-15 LPT 定址設定helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip22 4-1 系統架構流程圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip23 4-2 MATLAB 結構關係圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip25 4-3 功能方塊總集helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-4 進入 Simulink 按鈕helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip26 4-5 進入mdl 設計區helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip27 4-6 控制器設計連結圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip33 4-7 Q1 關節角度定位誤差 helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip 34 4-8 Q2 關節角度定位誤差helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip35 4-9 Q1 關節角度定位誤差（m1=0185kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-10 Q2 關節角度定位誤差（me=0122kg）helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip36 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件hellip38 4-12 NET 允許不同程式語言於使用受限的環境helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip39 4-13 於「啟始頁」中設定偏好設定值helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-14 dll 檔轉換設定示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip41 4-15 軟體架構示意圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip42 4-16 軟體控制介面helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip43 4-17 機械手臂外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip44 4-18 機械蟲外觀結構圖helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip45

vii

表目錄 2-1 機械手臂各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip8 3-1 脈波寬度與角度關係表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-2 伺服馬達規格helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-3 伺服馬達步階規格表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 4-1 S-Function 各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip29

1

第一章 緒論

11 前言

近年來半導體業者不斷的致力於無人操控機台及機台人員操控設備的自動化

在無人操控的狀態下機械手臂的操控必需相對提升其穩定度及靈活性才能準確

的完成電子電路零組件的鋪線 (如自動焊接機台點焊搬運機器手臂多用途搬

運機器人焊接切割機器人)在高精密度需求的半導體製程公司手臂控制系統必

須具有高度的準確性(如使用機器手臂將 IC 零件抓取放置於自動焊接機台的 PCB

模組的正確位置讓 PCB 板上完成點焊的動作)而在具有高度危險的工作環境中(如

高幅射的機台設備及高放射性醫療儀器等)也須藉助機器手臂來完成特定任務

所以機器手臂的操控與研究變得非常重要對需要手動操控的機台設備操控者也

能更方便的使用此項設備由此可知機器手臂應用範圍很廣泛由於精確性及穩

定性的要求如何降低系統內部與外部的不確定性及干擾對系統的影響因此對

於機械手臂之應用系統也就變得很重要

12 研究動機與方法

在業界中伺服馬達常使用 PI 控制器來調整直流無刷馬達的電流控制迴路

並需自行調整 PI 控制器參數使得電流迴路擁有符合實際動作的性能需求但本

研究考慮到（1）物理的運動方程式與運動產生的慣量（2）因牛頓運動第二定律 F

= ma 手臂擺動受到加速度的影響易產生的不穩定情況（3）機械手臂向下擺動

的向心力因此建立機械手臂所需的 PD 控制器在角度定位控制則是以三維

空間做為機械手臂所能延展的運動方向及考慮到機械手臂上的伺服馬達由下至

上的擺動所產生的加速度最後實體系統所連結的6顆伺服馬達將由控制器的設

計並透過Simulink及Visual Studio 2003 軟體上的設計來取得六顆伺服馬達的平衡

點在軟體上所設定的參數則尚需微調手臂上的零件與軟體上所設定的角度定位

參數吻合直至能準確定位於目標位置

2

利用 PD 控制器來控制機械手臂優點為可增大阻尼比提高系統穩定度使

用其來直接定位缺點是如果受到外在環境的影響如受到負載增加受外力干擾

等因素所定位的角度則會產生些微偏差而不能自我調整如果改良伺服馬達的

構造於伺服馬達內部的前端加裝熱感應器採用 PID 控制器來設計定位控制可

使系統增加強健性的功能受到外力的影響時感應器接收到感應定位的位置有些

微偏差而能自動調整角度至定位

在本論文中我們將討論機械手臂在具有 6 個自由度的方向的控制利用向量

轉動的方式分析其穩定性並採用線性系統的解析方式及 Lyapunov 穩定方法來討論

此系統[8][10]吾人將採用的物理向量及控制系統模型[13][22][23]根據手臂的運

動方式討論其動態方程式將手臂模型線性化可參考文獻[4][5]而透過手臂本身具

有向下引力等物理現象來推導出公式並驗證

13 論文架構

第一章為本論文的簡介以簡短的方式讓讀者了解到機械手臂的初步概念及應

用技術面的探討由第二章開始考慮到如何將它實現並考慮成本上的問題

採用 DC 伺服馬達來做控制控制器的設計是使用 PD 控制器來將它實現做為定位

控制並且介紹所使用的數學模型的推算及驗證

第三章的部份則是如何實現將 DC 伺服馬達應用於連桿裝置做為機械手臂的

前臂使用伺服馬達的齒輪的互相連結可以達成可彎曲的動作並且由以下章節

逐一介紹機台的建立與所需要的工作環境及傳輸的模式

第四章的部份則介紹在軟體上的所需要做的規劃如何將硬體的部份透過傳

輸的方式編寫語言程式建立軟體並控制硬體透過軟體規劃流程表做為此系統軟

體的架構使軟體與硬體能於同步進行做控制並能讀取即時的數據做變更能馬上

讀取數據資料而硬體的部份能即時做轉換整個實驗過程結果做一整理並配合

3

圖片數據說明本論文的實際動作提供往後的相關人員作為可用的經驗與數據

最後一章將討論分析本論文的實驗過程研究心得以及未來方向並以簡短的

方式紀錄這一年半的實驗心得與感想緊接著帶出專題往後的發展與後續可研究的

方向並以商業行為的角度試著提出往後可行性的穩健的技術為發展方向作為

本論文的結尾

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

vii

表目錄 2-1 機械手臂各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip8 3-1 脈波寬度與角度關係表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip17 3-2 伺服馬達規格helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 3-3 伺服馬達步階規格表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip19 4-1 S-Function 各類參數表helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellip29

1

第一章 緒論

11 前言

近年來半導體業者不斷的致力於無人操控機台及機台人員操控設備的自動化

在無人操控的狀態下機械手臂的操控必需相對提升其穩定度及靈活性才能準確

的完成電子電路零組件的鋪線 (如自動焊接機台點焊搬運機器手臂多用途搬

運機器人焊接切割機器人)在高精密度需求的半導體製程公司手臂控制系統必

須具有高度的準確性(如使用機器手臂將 IC 零件抓取放置於自動焊接機台的 PCB

模組的正確位置讓 PCB 板上完成點焊的動作)而在具有高度危險的工作環境中(如

高幅射的機台設備及高放射性醫療儀器等)也須藉助機器手臂來完成特定任務

所以機器手臂的操控與研究變得非常重要對需要手動操控的機台設備操控者也

能更方便的使用此項設備由此可知機器手臂應用範圍很廣泛由於精確性及穩

定性的要求如何降低系統內部與外部的不確定性及干擾對系統的影響因此對

於機械手臂之應用系統也就變得很重要

12 研究動機與方法

在業界中伺服馬達常使用 PI 控制器來調整直流無刷馬達的電流控制迴路

並需自行調整 PI 控制器參數使得電流迴路擁有符合實際動作的性能需求但本

研究考慮到（1）物理的運動方程式與運動產生的慣量（2）因牛頓運動第二定律 F

= ma 手臂擺動受到加速度的影響易產生的不穩定情況（3）機械手臂向下擺動

的向心力因此建立機械手臂所需的 PD 控制器在角度定位控制則是以三維

空間做為機械手臂所能延展的運動方向及考慮到機械手臂上的伺服馬達由下至

上的擺動所產生的加速度最後實體系統所連結的6顆伺服馬達將由控制器的設

計並透過Simulink及Visual Studio 2003 軟體上的設計來取得六顆伺服馬達的平衡

點在軟體上所設定的參數則尚需微調手臂上的零件與軟體上所設定的角度定位

參數吻合直至能準確定位於目標位置

2

利用 PD 控制器來控制機械手臂優點為可增大阻尼比提高系統穩定度使

用其來直接定位缺點是如果受到外在環境的影響如受到負載增加受外力干擾

等因素所定位的角度則會產生些微偏差而不能自我調整如果改良伺服馬達的

構造於伺服馬達內部的前端加裝熱感應器採用 PID 控制器來設計定位控制可

使系統增加強健性的功能受到外力的影響時感應器接收到感應定位的位置有些

微偏差而能自動調整角度至定位

在本論文中我們將討論機械手臂在具有 6 個自由度的方向的控制利用向量

轉動的方式分析其穩定性並採用線性系統的解析方式及 Lyapunov 穩定方法來討論

此系統[8][10]吾人將採用的物理向量及控制系統模型[13][22][23]根據手臂的運

動方式討論其動態方程式將手臂模型線性化可參考文獻[4][5]而透過手臂本身具

有向下引力等物理現象來推導出公式並驗證

13 論文架構

第一章為本論文的簡介以簡短的方式讓讀者了解到機械手臂的初步概念及應

用技術面的探討由第二章開始考慮到如何將它實現並考慮成本上的問題

採用 DC 伺服馬達來做控制控制器的設計是使用 PD 控制器來將它實現做為定位

控制並且介紹所使用的數學模型的推算及驗證

第三章的部份則是如何實現將 DC 伺服馬達應用於連桿裝置做為機械手臂的

前臂使用伺服馬達的齒輪的互相連結可以達成可彎曲的動作並且由以下章節

逐一介紹機台的建立與所需要的工作環境及傳輸的模式

第四章的部份則介紹在軟體上的所需要做的規劃如何將硬體的部份透過傳

輸的方式編寫語言程式建立軟體並控制硬體透過軟體規劃流程表做為此系統軟

體的架構使軟體與硬體能於同步進行做控制並能讀取即時的數據做變更能馬上

讀取數據資料而硬體的部份能即時做轉換整個實驗過程結果做一整理並配合

3

圖片數據說明本論文的實際動作提供往後的相關人員作為可用的經驗與數據

最後一章將討論分析本論文的實驗過程研究心得以及未來方向並以簡短的

方式紀錄這一年半的實驗心得與感想緊接著帶出專題往後的發展與後續可研究的

方向並以商業行為的角度試著提出往後可行性的穩健的技術為發展方向作為

本論文的結尾

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

1

第一章 緒論

11 前言

近年來半導體業者不斷的致力於無人操控機台及機台人員操控設備的自動化

在無人操控的狀態下機械手臂的操控必需相對提升其穩定度及靈活性才能準確

的完成電子電路零組件的鋪線 (如自動焊接機台點焊搬運機器手臂多用途搬

運機器人焊接切割機器人)在高精密度需求的半導體製程公司手臂控制系統必

須具有高度的準確性(如使用機器手臂將 IC 零件抓取放置於自動焊接機台的 PCB

模組的正確位置讓 PCB 板上完成點焊的動作)而在具有高度危險的工作環境中(如

高幅射的機台設備及高放射性醫療儀器等)也須藉助機器手臂來完成特定任務

所以機器手臂的操控與研究變得非常重要對需要手動操控的機台設備操控者也

能更方便的使用此項設備由此可知機器手臂應用範圍很廣泛由於精確性及穩

定性的要求如何降低系統內部與外部的不確定性及干擾對系統的影響因此對

於機械手臂之應用系統也就變得很重要

12 研究動機與方法

在業界中伺服馬達常使用 PI 控制器來調整直流無刷馬達的電流控制迴路

並需自行調整 PI 控制器參數使得電流迴路擁有符合實際動作的性能需求但本

研究考慮到（1）物理的運動方程式與運動產生的慣量（2）因牛頓運動第二定律 F

= ma 手臂擺動受到加速度的影響易產生的不穩定情況（3）機械手臂向下擺動

的向心力因此建立機械手臂所需的 PD 控制器在角度定位控制則是以三維

空間做為機械手臂所能延展的運動方向及考慮到機械手臂上的伺服馬達由下至

上的擺動所產生的加速度最後實體系統所連結的6顆伺服馬達將由控制器的設

計並透過Simulink及Visual Studio 2003 軟體上的設計來取得六顆伺服馬達的平衡

點在軟體上所設定的參數則尚需微調手臂上的零件與軟體上所設定的角度定位

參數吻合直至能準確定位於目標位置

2

利用 PD 控制器來控制機械手臂優點為可增大阻尼比提高系統穩定度使

用其來直接定位缺點是如果受到外在環境的影響如受到負載增加受外力干擾

等因素所定位的角度則會產生些微偏差而不能自我調整如果改良伺服馬達的

構造於伺服馬達內部的前端加裝熱感應器採用 PID 控制器來設計定位控制可

使系統增加強健性的功能受到外力的影響時感應器接收到感應定位的位置有些

微偏差而能自動調整角度至定位

在本論文中我們將討論機械手臂在具有 6 個自由度的方向的控制利用向量

轉動的方式分析其穩定性並採用線性系統的解析方式及 Lyapunov 穩定方法來討論

此系統[8][10]吾人將採用的物理向量及控制系統模型[13][22][23]根據手臂的運

動方式討論其動態方程式將手臂模型線性化可參考文獻[4][5]而透過手臂本身具

有向下引力等物理現象來推導出公式並驗證

13 論文架構

第一章為本論文的簡介以簡短的方式讓讀者了解到機械手臂的初步概念及應

用技術面的探討由第二章開始考慮到如何將它實現並考慮成本上的問題

採用 DC 伺服馬達來做控制控制器的設計是使用 PD 控制器來將它實現做為定位

控制並且介紹所使用的數學模型的推算及驗證

第三章的部份則是如何實現將 DC 伺服馬達應用於連桿裝置做為機械手臂的

前臂使用伺服馬達的齒輪的互相連結可以達成可彎曲的動作並且由以下章節

逐一介紹機台的建立與所需要的工作環境及傳輸的模式

第四章的部份則介紹在軟體上的所需要做的規劃如何將硬體的部份透過傳

輸的方式編寫語言程式建立軟體並控制硬體透過軟體規劃流程表做為此系統軟

體的架構使軟體與硬體能於同步進行做控制並能讀取即時的數據做變更能馬上

讀取數據資料而硬體的部份能即時做轉換整個實驗過程結果做一整理並配合

3

圖片數據說明本論文的實際動作提供往後的相關人員作為可用的經驗與數據

最後一章將討論分析本論文的實驗過程研究心得以及未來方向並以簡短的

方式紀錄這一年半的實驗心得與感想緊接著帶出專題往後的發展與後續可研究的

方向並以商業行為的角度試著提出往後可行性的穩健的技術為發展方向作為

本論文的結尾

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

2

利用 PD 控制器來控制機械手臂優點為可增大阻尼比提高系統穩定度使

用其來直接定位缺點是如果受到外在環境的影響如受到負載增加受外力干擾

等因素所定位的角度則會產生些微偏差而不能自我調整如果改良伺服馬達的

構造於伺服馬達內部的前端加裝熱感應器採用 PID 控制器來設計定位控制可

使系統增加強健性的功能受到外力的影響時感應器接收到感應定位的位置有些

微偏差而能自動調整角度至定位

在本論文中我們將討論機械手臂在具有 6 個自由度的方向的控制利用向量

轉動的方式分析其穩定性並採用線性系統的解析方式及 Lyapunov 穩定方法來討論

此系統[8][10]吾人將採用的物理向量及控制系統模型[13][22][23]根據手臂的運

動方式討論其動態方程式將手臂模型線性化可參考文獻[4][5]而透過手臂本身具

有向下引力等物理現象來推導出公式並驗證

13 論文架構

第一章為本論文的簡介以簡短的方式讓讀者了解到機械手臂的初步概念及應

用技術面的探討由第二章開始考慮到如何將它實現並考慮成本上的問題

採用 DC 伺服馬達來做控制控制器的設計是使用 PD 控制器來將它實現做為定位

控制並且介紹所使用的數學模型的推算及驗證

第三章的部份則是如何實現將 DC 伺服馬達應用於連桿裝置做為機械手臂的

前臂使用伺服馬達的齒輪的互相連結可以達成可彎曲的動作並且由以下章節

逐一介紹機台的建立與所需要的工作環境及傳輸的模式

第四章的部份則介紹在軟體上的所需要做的規劃如何將硬體的部份透過傳

輸的方式編寫語言程式建立軟體並控制硬體透過軟體規劃流程表做為此系統軟

體的架構使軟體與硬體能於同步進行做控制並能讀取即時的數據做變更能馬上

讀取數據資料而硬體的部份能即時做轉換整個實驗過程結果做一整理並配合

3

圖片數據說明本論文的實際動作提供往後的相關人員作為可用的經驗與數據

最後一章將討論分析本論文的實驗過程研究心得以及未來方向並以簡短的

方式紀錄這一年半的實驗心得與感想緊接著帶出專題往後的發展與後續可研究的

方向並以商業行為的角度試著提出往後可行性的穩健的技術為發展方向作為

本論文的結尾

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

3

圖片數據說明本論文的實際動作提供往後的相關人員作為可用的經驗與數據

最後一章將討論分析本論文的實驗過程研究心得以及未來方向並以簡短的

方式紀錄這一年半的實驗心得與感想緊接著帶出專題往後的發展與後續可研究的

方向並以商業行為的角度試著提出往後可行性的穩健的技術為發展方向作為

本論文的結尾

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

4

第二章 機械手臂控制理論介紹

21 機械手臂系統模型

211 動態方程式

先考慮一機械手臂於二維平面上的運動其位置可由關節角度向量 來表示

設定二維力矩向量輸入為 其非線性動態方程式為

(2-1) 其中

關節角度向量

關節角速度向量

關節角加速度向量

機械手臂慣量矩陣

( ) 向心力及科氏力矩向量

重力力矩向量

從物理運動學的角度慣性矩陣 決定於關節定位位置 (如從人體肩膀的角

度延伸手臂比起折疊手臂具有更大的慣量)向心力矩隨關節角度的平方而異科

氏力則是與兩個關節的乘積有關機械手臂的動能公式

(2-2)

為正值因為動能對於任何關節角度位置 和任何非零關節角速度 都必須為正

值由此可知 為正定亦即存在一常數 α gt 0使得對於機器手臂的運

動空間的所有位置需要

α (2-3)

其中 為單位矩陣反觀得知運動的空間就需存在一位置向量 而在此位置上

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

5

慣性矩陣俱有零的特徵值這是因為工作的空間本身為一閉集合假設 是與零

特徵值有關的特徵向量機器手臂就能以單位速度 )移動而動能為零

這顯然不可能的因此 必為正定

212 手臂系統模型

如圖 2-1 所示假設該機械手臂位於水平面上( 目的為將手臂移

動至預定的最終位置這個位置將由目標關節角度 dq 來決定而關節的比例-微分控

制器 (PD controller)即根據各關節測量到的位置誤差 和速度

1 2 其中 第 j 個關節角度

第 j 個關節角速度

第 j 個關節位置誤差

第 j 個關節位置誤差速度

第 j 個關節預定角度

代入以下公式來產生各關節機構輸入的回授控制律

(2-4)

圖 2-1 機械手臂示意圖

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

6

以完成目標位置的定位控制其中 和 為正值

以能量守恆為原則將動態方程式改寫如下

]= (2-5)

若以手臂本身的能量為輸入將機械手臂的動能微分由於慣量矩陣 的時變性

科氏力和向心力並未消失將可推論得到 PD 控制器的穩定性和收斂性

將(2-4)化簡如下

(2-6)

其中 和 為正定矩陣

在物理上若使用彈簧和阻尼將(2-6)式實現則考慮與這系統有關的總機械能

] (2-7)

將總機械能 視為 Lyapunov 函數配合(2-5)式取其對時間的導數可以得到

(2-8)

利用(2-6)式代入上式推導為

=

=

=

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

7

得到下列公式

0 (2-9)

可以將 視為阻尼器所消耗的功率由此可得知系統將可收歛於期望的狀態

22 PD 控制器

221 機械手臂定位控制

利用前一節對 PD 控制器的討論[1][10]進行機械手臂的定位控制[11][20][22]將角度向量 設為

(2-10)

輸入設為

(2-11)

將(2-10)式及(2-11)式套入系統方程式(2-1)式得到

+ 0 (2-12)

其中

2 2

和

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

8

代入表 2-1 之數值將 及 角度設定於0度位置 為目標角度若給定欲定位

之角度 0和 90選擇的增益值如下

100

20 其中 為 2 2 單位矩陣

得到 2000

總能量為

]

將 微分 0

根據 Lyapunov 定理保證系統穩定

表 2-1 機械手臂各類參數表

符號 數值 單位 名稱解釋

lm 0185 kg 第一支手臂質量

em 0122 kg 第二支手臂質量

1l 75 cm 第一支手臂長度

eδ 90 度 目標定位角度

1cl 32258 cm 第一支手臂中心與第二

支手臂所支撐的距離

cel 7493 cm 第一支手臂長度

1I 02 cmradsec 第一支手臂轉動慣量

eI 049 cmradsec 第二支手臂轉動慣量

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

9

第三章 硬體架構說明

31 硬體架構

本實驗以一台電腦並透過印表機的通訊埠 來連結 Robix 公司所生產的驅動器

來控制伺服馬達如下圖

圖 3-1 硬體架構流程圖

32 機械手臂

本實驗所製作的機械手臂是使用 Robix 公司所生產的零件組裝而成可透過

工具箱裡面的零件設計自己所需的仿真機械手臂外觀工具箱的零件組如下

圖 3-2 零件明細

Computer

Robix Driver

LPT Port

Robot

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

10

此組合零件的彈性極大可完全依照使用者的需求組裝出所需的機械轉軸軸

數軸向軸長因為零件是由鋁片來連接伺服馬達並且採用塑膠螺帽使得本

身結構較為輕巧固定零件(如連結鋁片小螺帽塑膠齒輪hellip等)的減輕可減少

本身手臂運動所產生的重量(如重力向心力hellip等)幫助伺服馬達能夠快速定位

盡量減少不穩定的產生

321 N 自由度介紹[21]

在統計學上自由度（Degree Of Freedom DOF）是指當以樣本的統計量來估計

總體的參數時 樣本中能自己獨立或能自由變化的資料的個數稱為該統計量的自

由度 n

例如在估計總體的平均數時樣本中 n 個數全部加起來其中任何一個數都

和其他資料相互獨立從其中抽出任何一個數都不影響其他資料（這也是隨機抽

樣所要求的） 因此一組資料中每一個資料都是獨立的所以自由度就是估計總體

參數時獨立資料的數目而平均數是根據 n 個體獨立資料來估計的因此自由度為

n

在物理學裡自由度所指的是物理系統的獨立坐標的個數力學系統由一組坐

標來描述比如一個質點的三維空間中的運動在笛卡爾坐標系中由 三

個坐標來描述或者在球坐標系中由 γθψ三個坐標描述描述系統的坐標可

以自由的選取但獨立坐標的個數總是一定的即系統的自由度一般的N 個質

點組成的力學系統由 3N 個坐標來描述但力學系統中常常存在著各種約束使得

這 3N 個坐標並不都是獨立的對於 N 個質點組成的力學系統若存在 m 個完整

約束則系統的自由度減為

S=3N m 例如在一個平面上運動的一個質點其自由度為 2 又或是在空間中的兩個質

點中間以線連接所以其自由度

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

11

S=3 2 1=5 ( 2 個質點有 3 個位移方向但具有一條線所形成的約束)

除了平移的自由度外還有轉動及振動自由度

約束在古典力學裏物體的運動必須遵守牛頓運動定律除此以外每一個物理

系統時常會有一些約束物體的運動也必須遵守這些約束例如單擺系統

的約束是擺繩的長度是常數擺錘與支撐點的距離必須是這長度除非水瓶

破了一個封閉的水瓶裏的水分子絕對不能運動到水瓶的外面這些約束

使物理系統的特性呈現出來我們要分析一個物理系統必須了解這系統裏

的約束

因為約束的作用我們在分析物體的運動上會遇到一些新的困難

a 許多描述物體運動的位置不再互相獨立如果這約束是完整約束我們可以用廣

義坐標來除去一些相依的位置如果整個系統是完整系統我們可以用獨立的廣

義坐標來表示這個系統的運動時常我們可以找到相關的形式論來分析這個系統

的運動

b 假若一個物體的運動因為約束而改變則必定有一個關於這約束的力作用於這物

體上不然這物體的運動不會改變稱此力為約束力一般而言我們事先並

不知道約束力的值量如果我們能將一個系統所有的廣義坐標都轉換成互不相依

的廣義坐標那麼我們不需要知道約束力就可以求得物體的運動方程式了

c 約束可以分為完整約束及非完整約束兩種類型

完整約束可用方程式表示為

Ｎｔ ＝０

其中 為每一粒子 之位置 和時間 之函數

非完整約束若一約束不能夠用上述方程式表示則稱此約束為非完整約束

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

12

在古典力學裏如果一個系統的所有系統約束都是完整約束則稱此系統為完整系

統完整約束又稱為完整限制表示為

Ｎｔ ＝０

其中 為每一個粒子 之位置 和時間 的函數

若一約束不能夠用上述方程式表示則稱此約束為非完整約束

322 外觀模型建立

a將所需零件設計成如下草圖

圖 3-3 機械手臂組裝機構圖

b一開始使用者將從台面的結構組合起將主軸結構立於主體平台上如圖所示

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

13

圖 3-4 機械手臂基底組立

c再將驅動主軸的伺服馬達架上並鎖上支撐架軸關節及軸臂如圖 3-5 所示

圖 3-5 伺服馬達組立

d再依上敘組裝的動作並參考圖 3-3 組合各式零件伺服馬達共 6 顆由鋁片及塑

膠螺帽栓緊固定底盤將由鐵條固定組合完成如下圖所示

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

14

圖 3-6 機械手臂組合完成圖

e將伺服馬達所連接的電線經過軸臂整線於整線槽內並拉至於基底

圖 3-7 伺服馬達與驅動器整線完成圖

f將電線依序插上伺服馬達驅動電路卡

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

15

圖 3-8 伺服馬達與驅動器連結

g將電路卡上的訊號纜線接至於電腦的 Printer Port以上硬體結構則完成

323 伺服馬達介紹

所採用的伺服馬達是採用 Hitec 公司所生產的伺服馬達其採用脈波寬度來控制

伺服馬達一個微型伺服馬達內部包含了一個小型直流馬達一組變速齒輪組一

個回饋可調電位器及一塊電子控制板其中高速轉動的直流馬達提供了原始動力

帶動變速（減速）齒輪組使之產生高扭力的輸出齒輪組的變速比愈大伺服馬

達的輸出扭力也愈大也就是說越能承受更大的重量但轉動的速度也愈低

圖 3-9 伺服馬達內部結構圖

變速齒輪組

可調電位器

電子控制板

微型直流馬達

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

16

控制脈衝

微型伺服馬達是一個閉迴路回饋系統其原理可由下圖表示

圖 3-10 伺服馬達閉迴路回饋系統

減速齒輪組將由馬達驅動其輸出端傳動一個線性的比例電位器作位置檢測

該電位器把轉角座標轉換為一比例電壓回饋給控制電路板控制電路板將其與輸

入的控制脈衝信號做比較產生修正脈衝信號並驅動馬達正向或反向地轉動使

齒輪組的輸出位置與期望值相符令修正脈衝訊號設為 0因此使伺服馬達精確

達到定位的目的

標準的微型伺服馬達有三條控制線分別為訊號電源線(+)及電線(-)電源

線與地線用於提供內部的直流馬達及控制線路所需的電壓及電流電壓通常介於

4Vmdash6V之間該電源應盡可能與處理系統的電源隔離（因為伺服馬達會產生噪音）

甚至小伺服馬達在重負載時也會降低放大器的電壓所以整個系統的電源供應的比

例必須正常合理

輸入一個週期性的正向脈衝信號這個週期性脈衝信號的高電壓平均時間通常

在 1msmdash2ms 之間而低電壓平均時間應在 5ms 到 20ms 之間表 3-1 示出一個典型

的 20ms 週期性脈衝的正脈衝寬度與微型伺服馬達的輸出手臂位置的關係在此

本實作則設定輸入訊號為 5ms

控制電路

馬達

齒輪組

比例

電位器

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

17

表 3-1 脈波寬度與角度關係表 輸入正脈衝寬度 (週期為 20 ms本實作輸入訊號 5 ms )

伺服馬達輸出定位角度

05ms

10ms

15ms

20ms

25ms

電源線有三條如圖 3-11 所示

腳位說明伺服馬達三條線中紅色的線是控制線接到控制晶片上中間是 SERVO

工作電源線一般工作電源是 5V電壓範圍為 4V~6V 第三條則是地線

圖 3-11 伺服馬達接線示意圖

伺服馬達的運動速度其暫態運動速度是由其內部的直流馬達和變速齒輪組的

配合決定的在固定的電壓驅動下其數值唯一但其平均運動速度可通過分段停

頓的控制方式來改變例如我們可把動作幅度為 90ordm 的轉動細分為 128 個停頓點

通過控制每個停頓點的時間長短來實現 0ordmmdash90ordm 變化的平均速度對於多數伺服馬達

來說速度的單位由ldquo度數秒rdquo來決定

訊號輸入說明訊號是由脈波的寬度來調整它的轉向脈波週期為 5ms

９０

４５

０

４５

９０

訊號輸入

電源輸入(+5)

接地線

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

18

例如設定 15ms(Hi)05ms(Lo)伺服馬達轉向為中間16ms(HI)04(LO)伺服馬達轉

向左邊一格14ms(HI)06ms(LO)伺服馬達轉向右邊一格其他的轉向就以此類推

使用伺服馬達的注意事項

a除非使用的是數位式的伺服馬達否則以上的伺服馬達輸出臂位置只是一個不準

確的大約數

b普通的模擬微型伺服馬達不是一個精確的定位器件即使是使用同一品牌型號的

微型伺服馬達產品他們之間的差別也是非常大的在同一脈衝驅動時不同的伺

服馬達存在plusmn10ordm 的偏差也是正常的

c正因上述的原因不推薦使用小於 1ms 及大於 2ms 的脈衝作為驅動信號實際上

伺服馬達的最初設計表也只是在plusmn45ordm 的範圍而且超出此範圍時脈衝寬度轉動

角度之間的線性關係也會變差

d不可載入讓伺服馬達輸出位置超過plusmn90ordm 的脈衝信號否則會損壞伺服馬達的輸出

限位機構或齒輪組等機械部件

e由於伺服馬達的輸出位置角度與控制信號脈衝寬度沒有明顯統一的標準而且其

行程的總量對於不同的廠家來說也有很大差別所以控制軟體必須具備有依據不同

伺服馬達進行單獨設置的功能

以下伺服馬達則是採用 Hitec 公司所生產的 HS-422 的規格( 表 3-2 )依據 Hitec 公

司的零件規格表來設計所需轉動的角度大小軟體驅動則是依據表 3-3 所示來設

定軟體的前置設定

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

19

表 3-2 伺服馬達規格

圖 3-12 HSS-422 伺服馬達外觀

表 3-3 伺服馬達步階規格表 Servo Motor Min Max

SoftWare Steps

HardWare Steps

Working Angle

-1400 1400

560 steps 100deg 100deg

324 LPT( Line Printer Terminal )通訊協定介面

早期的 286386 電腦外接介面還未如同今日介面上的種類繁多COM1 與 LPT

應該是最普及和用途最廣的介面設計它最早的版本誕生在 1981 年如今即將邁入

第 27 個年而依然能歷久不衰被工程師稱為『摩爾奇蹟』LPT 不單是最早應用在

印表機介面它也可以是單機的網路線掃描器ZIP-軟碟軟體保護器等各式各

樣的設計都在 LPT 上被開發過這得感謝 IBM 不但公開其架構且開放性的定址

設計如278 378 - 出廠開機設定 和 3BC (Hex 編碼)幾乎已經是分配鐵律般地固

著於一般的 PC 之中讓 LPT 持續在系統資源裡獨占鼇頭

Standard Printer Ports (SPP) EPPECP

由於早期電腦 4040 系列對傳輸速率沒有太多的要求初始的 LPT 通道只有

8bit 150KSec半雙工單向傳輸的能力這個標準隨後被 IBM 納入 XTAT 的標準

規格稱為 Standard Printer Ports (SPP)LPT 被分割為 8 個資料流（ data lines）8

HS 422 規格詳述

電壓 轉矩 速度

48 33Kgcm 021sec60 deg

60 41Kgcm 016sec60 deg

零件外觀大小(mm) 重量

406 x 198 x 366 455g

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

20

個接地流（ handshaking lines）4 個輸出流（output）和 5 個 輸入流（input)(圖 39)

隨後這個規格被加上了相容資料讀取能力成為第一代的 PS2 所引用的規格稱為

Bi-directional parallel port (BPP)

圖 3-13 並列埠模式設定

延續最早期的設定往後的 PC 幾乎都將 LPT 平行介面列為標準配備同時

也確認出廠設定 IRQ7 為其標準中斷值為了因應 PC 邁入更高的速度（486 時代

來臨）除了 SPP 標準工作模式外新一代 EPP( Enhanced Parallel Port )增強型工作

模式因而誕生EPP 採用雙向半雙工資料傳輸其傳輸速度比 SPP 高可達 2MBs

相當於 USB10 的傳輸速率不過由 EPP 於開發時正好適逢 PC 百家爭鳴時代

業界面對相容性問題時因而將 EPP 模式後又細分為 EPP17 和 EPP19 兩種許多

後續依靠 LPT 介面開發之外接硬碟盒與其他相關周邊設備均相容此一工作模式

至於ECP( Extended Capabilities Port)擴充型工作模式則完全採用雙向全雙工資料

傳輸傳輸速率比 EPP 稍高但不是每一種 LPT 設備都支援此一模式因而常造

成設備之錯亂

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

21

圖 3-14 LPT 各接點串流位址對應

一般來說 EPP 並非支援印表機為主而是外接式 CD ROM硬碟盒以及點

對點網路為主 ECP才是主要瞄準新一代印表機與掃描器之規格不過隨著雙方

版本不斷的演進和 USB 的誕生這之間的界線也就越來越模糊了

規格制訂和標準執行都是非常嚴格的一件事隨著 SPP 和 BPP 規格的推出市

面上開始出現一些以此為基準卻又不完全相容的設計為了避免社會成本的浪費

美國工業標準局特別制訂 IEEE 1284 標準規範整套系統由於 IEEE1284 的介入

不僅 LPT 有了公定的連接標準通信頻寬更重要的是統一而嚴格制訂下有效的將

LPT 的纜線由原先的 6 呎長延伸到 30 呎更增添了 LPT 的用途

而本實驗之伺服馬達驅動器與電腦所連結的介面為 Printer Port要啟動驅動器

控制伺服馬達需將驅動程式在控制台設定從印表機模式切為 Robot controllers

模式IO 位址設定為 0x378LPT1 設定為 IRQ7 如此才能正常交握如圖 3-16 所

示

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

22

圖 3-15 LPT 定址設定

由上圖得知將會透過 LPT1 來傳輸則傳輸的驅動程式則是使用 Robix 公司所

提供的驅動程式來源碼IO 設定範圍為 0x378-0x37F當交握成功後則圖上不會

產生錯誤訊息 的圖示

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

23

第四章 軟體規劃

41 軟體架構說明

本論文所實驗的軟體主要將透過 Matlab 軟體本身內建的 Simulink 來撰寫 PD

控制器的程式所得到的數據資料由 S-Function 轉換成m 程式頁面數據資料再傳

到到由 Visual Studio 2003 程式所撰寫的手臂定位程式來將以定位伺服馬達各種

馬達互相配合再轉動至預先設定的角度位置則系統架構流程圖如下

圖 4-1 系統架構流程圖

Visual Studio 2003 程式

Computer

LPT Port

Servo Motor Driver

Servo Motor

Matlab 程式

Data

Compiler

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

24

42 Matlab 環境介紹[3]

Matlab 是一種效率高的工程語言可以進行資料分析模型建立模擬硬體

控制以便完成原型之演算法發展發展過程中很容易藉由易於使用的圖型表現能

力將結果顯現出來再配合除錯器的功能讓使用者很容易進行除錯縮短發展

演算法的時間

MATLAB 與 C 語言最大的不同在於不須編譯的過程而是採用直譯的方式來

進形程式碼的解讀因此我們可以在 MATLAB 的 command windows 中直接下達指

令而經由其運算位元將運算結果顯現出來同理如果把很多的 script 集合起來按

照執行的順序放進同一個資料夾我們就可一次命令 MATLAB 執行完畢所有在

M-File 中的 script如果該一 M-File 沒有錯誤自然將結果顯現出來如果有錯就

得依靠經驗或經由編譯器的除錯功能一步一步的驗證將出錯誤的 script 找出這

樣的程式開發環境很容易讓程式編寫人員將思緒集中在數學的通理和邏輯判斷上

同時為了特定的工程領域所發展的 M-File集中起來便成該領域的 ToolBox可以

傳承與擴散的累積經驗因此 MATLAB 在學術領域醫療領域及工程領域上獲得很

大的支持目前的 ToolBox 已經累積至數十種且仍在增加中

如果把研究目標MATLAB 與人的關係予以剖析則發現有以下關聯性

MATLAB 提供一個發展的環境 (Development Environment) 給人指令然後將自己

推導的演算法依序寫成一串 script 的 M-File 告知 MATLAB MATLAB 再根據指

令去抓取函數庫(Mathematical Function Library)中的資源或透過別的語法所寫的

應用程式介面(Application Program Interface(API)) 在 MATLAB 語法中予以運算最

後再把圖形顯現出來(圖 4-2)

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

25

應用程式

介面

MATLAB

語法

圖 4-2 MATLAB 結構關係圖

421 Simulink 工作環境[3]

Simulink 為 MATLAB 環境下的模擬工具檔案類型為mdlSimulink 的介面提

供給使用者很方便的圖形化的功能方塊圖能夠簡單的建構出系統模擬的系統將

所設計的流程以圖樣的方式方便程式撰寫者便於觀看系統方塊及運作流程以便

於除錯並減輕設計上盲點更重要的是能夠以 MATLAB 語言C 語言FORTRAN

語言依據 S-Fuction 的標準格式寫成使用者自行定義的功能方塊因此使得擴充

性很高並且能夠與dll 檔案類型之應用程式做溝通具有很高的整合功能

函式庫

發展環境

圖形

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

26

所以當有些軟體會提供dll 檔的 S-Fuction 軟體以方便在 DDE( Dynamic Data

Exchan ger )時能夠與 Simulink 在 Command Windows 下執行 Simulink 或在Comm

and Windows 中的 Simulink 按鈕 (圖 4-4)即可出現 Simulink 的功能方塊總集(圖

4-3)與空白的設計區(圖 4-5)空白區為 Simulink 所設計的地方也可能從方塊總集

中由 File-gtNew-gtModel(圖 4-6)而得到

在方塊總集中的功能槽如 sourcelink等每個功能槽中有很多功能槽使

用者設計之用如果要設計流程將功能方塊放入空白區域將自行設計系統架構建

立草稿後組合完成後即可執行程式

圖 4-3 功能方塊總集

]

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

27

圖 4-4 進入 Simulink 按鈕

圖 4-5 進入mdl 設計區

422 S-Fuction 介紹[3]

S Function( System Function )其功能為藉由 Matlab 或 C 語言的程式規劃設計

出可達成所要求的功能方塊圖S- Function 係由一個 sys 的參數控制所以首先將

寫好的程式在面板程式中輸入必要的副程式區撰寫程式以及輸入參數即可當程式

設計好後第二步就是到 User-Defined Function 功能槽複製 S-Function 系統功能方

塊進來然後把完成的程式檔名輸入以供其呼叫之

面板程式的呼叫的位置在 ToolBoxSimulinkBlocks 之中檔名為 sfuntmp1m

程式碼摘錄如下

function [sysx0strts] = sfuntmpl(txuflag)

switch flag

case 0

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

28

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

sizes = simsizes

sizesNumContStates = 0

sizesNumDiscStates = 0

sizesNumOutputs = 0

sizesNumInputs = 0

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = []

str = []

ts = [0 0]

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

29

function sys=mdlDerivatives(txu)

sys = []

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys = []

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

由上面的程式設定顯示主程式是 Simulink 模型的一個副程式只需要提供旗標

的工作再經由旗標設定去處理另外必備的六個副程式這六個副程式的內容即

是要給使用者設計的部份以下為其應用的時機

表 4-1 S-Function 各類參數表[3]

mdlInitializeSizes (Flag=0)

在 S Function 動作之前需設定其運作

架構此結構以 sizes 儲存然後再用

simsizes 指令去擷取 其架構細分六項 1 sizesNumContStates 連續狀態個數 2 sizesNumDiscStates 離散狀態個數 3 sizesNumoutputs 輸出個數 4 sizesNumInputs 輸入個數 5 sizesDirFeedthrough direct

deedthrought 的設定與否取決於輸

出是否為輸入的函數或是取樣頻率

是否為輸入函數 6 sizesNumSamoleTimes 一般設定為 1

mdlDerivatives 輸出值 sys 為狀態值的微分

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

30

(Flag=1) mdlUpdate (Flag=2)

輸出值 sys 為狀態值在下一時刻的更新

值

mdlOutputs (Flag=3)

輸出值 sys 為輸入值與狀態值的函數

mdlGetTimeOfNextVarHit (Flag=4)

輸出值 sys 為下一次被觸發的時間例

如sys=t+u(1)則輸入 u(1)的值為下一次

被觸發的時間 mdlTerminate (Flag=9)

模擬結束

Simulink 與 S-Function 的溝通參數有 4 個輸入為 txuflag

t為時間

x為狀態向量

u為輸入向量

Flag 則為 Simulink 執行何種動作的階斷旗標

另外傳回的參數值有 4 個為 sysx0strts

sys 為 S-Function 根據 Flag 的值運算出來的解

x0 為初始狀態

str 為m 檔案的 S-Function 的空矩陣

ts 為 2 行向量定義取樣時間及偏移值

以上即是對六個副程式的寫法至於 mdlGetTimeOfNextVarHitmdlUpdate 則

是運用在比較複雜的系統如有需要再參考 SIMULINK User Manual裡面將有詳細

的舉例說明

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

31

423 運用 Simulink 設計 PD 控制器

首先在 Simulink 的空白區域設計控制器的功能方塊圖則功能方塊圖的設定

將依照第二章所設計的控制器把每個運算的功能方塊圖建立好後做成一個個模

組( 如同在 MATLAB 的m 檔下寫的副程式 )再將每一個建立好的模組中的輸入向

量 u 定義好後依據先前建立好的數學公式放入即可對於將要在mdl 轉換m 檔的

格式則需透過 S-Function 來做為轉換則 S-Function 的設定如下

檔名為srobm

function [sysx0strts] = srob(txuflag)

switch flag

case 0

[sysx0strts]=mdlInitializeSizes

case 1

sys=mdlDerivatives(txu)

case 2

sys=mdlUpdate(txu)

case 3

sys=mdlOutputs(txu)

case 4

sys=mdlGetTimeOfNextVarHit(txu)

case 9

sys=mdlTerminate(txu)

otherwise

error([Unhandled flag = num2str(flag)])

end

function [sysx0strts]=mdlInitializeSizes

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

32

sizes = simsizes

sizesNumContStates = 2

sizesNumDiscStates = 0

sizesNumOutputs = 2

sizesNumInputs = 8

sizesDirFeedthrough = 1

sizesNumSampleTimes = 1

sys = simsizes(sizes)

x0 = [0 0]

str = []

ts = [0 0]

function sys=mdlDerivatives(txu)

a=[u(1) u(2)u(2) u(3)]

b=[u(4) u(5)u(6) 0]

ui=[u(7)u(8)]

sys=-inv(a)bx+inv(a)ui

function sys=mdlUpdate(txu)

sys = []

function sys=mdlOutputs(txu)

sys(1)=x(1)

sys(2)=x(2)

function sys=mdlGetTimeOfNextVarHit(txu)

sampleTime = 1

sys = t + sampleTime

function sys=mdlTerminate(txu)

sys = []

求解狀態的微分

輸出為角度的微分狀態

機械手臂的時間

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

33

Simulink程式設計如下

圖 4-6 控制器設計連接圖

透過 S-Function 的mdl 檔轉後m 檔後則能得到所需要的手臂定位資料將資料代

入下一節所要介紹的 Visual Studio 2003 軟體把資料讀入即能將操控手臂定位控

制

424 實驗結果

在實作部份手臂構造所採用的儀器則是使用 Robix 公司所生產的零件組所組

裝而成伺服馬達驅動器軔體則是採用 Rascal 0225 Beta 版軟體的撰寫則是使用

Visual Studio 2003電腦系統配備則是採用 Core 2 Duo 處裡器顯示卡則是採用

NVIDIA Geforce 7300 GT傳輸方式則是使用 Printer PortIO 位址設定為 0x378

硬體系統架構流程如圖 4-1 所示在 Matlab 中透過 Simulink 所設計的 PD 控制

器使用 S-function 將所產生的數據資料建立透過 Printer Port 將資料傳至伺服

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

34

馬達驅動器來控制伺服馬達調控機械手臂之角度至所定位的位置

觀察圖 4-7 及圖 4-8 的結果得知在圖 4-7 中時間 0 ~ 04 秒間擺動時的振

盪是因為第一支手臂需承載第二支手臂的重量以致於擺動振盪劇烈由於沒有

像第一支手臂負擔額外的重量因此圖 4-8 同樣在 0~04 秒時位置角度誤差 Q2

很快地趨近誤差為0再觀察在 04~2 秒之間的位置誤差角度 Q1 及 Q2Q2 是呈

現穩定的狀態 Q1 則還會產生些微的振盪

圖 4-7 Q1 關節角度定位誤差

0 02 04 06 08 1 12 14 16 18 2-15

-10

-5

0

5

10

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

35

圖 4-8 Q2 關節角度定位誤差

其次我們讓第一支手臂本身的重量固定為 ml=0185kg且 me =0122kg 當

第二支手臂負載 03kg 時則結果如圖 4-9 所示

另外我們再固定第二支手臂的重量 me =0122kg改變第一支手臂重量分別為

m1=0185kg 及 0485kg結果如圖 4-10 所示

觀察圖 4-9 及圖 4-10在 02~08 秒之間當第一支手臂重量增加至 0485kg

或第二支手臂增加負載 03kg 時Q1 比 Q2 需花費較多的時間以修正手臂的角度誤

差重量增加使得波動幅度越大所需定位的時間越長在 08~20 秒之間觀察 Q1

及 Q2 角度定位誤差於手臂所能承受重量的範圍內皆能在 2 秒內完成定位

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

36

圖 4-9 Q1 關節角度定位誤差（m1=0185kg）

圖 4-10 Q2 關節角度定位誤差（me=0122kg）

0 02 04 06 08 1 12 14 16 18 2-25

-20

-15

-10

-5

0

5

10

15

20

time(sec)

Q1

join

t ang

le e

rror(d

egre

e)

me=0122 kgme=0422 kg

0 02 04 06 08 1 12 14 16 18 2-90

-80

-70

-60

-50

-40

-30

-20

-10

0

10

time(sec)

Q2

join

t ang

le e

rror(d

egre

e)

m1=0185 kgm1=0485 kg

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

37

43 Visual Studio 2003 環境介紹[9]

一般而言微軟產品開發週期約為二年但 Microsoft Visual StudioNET 卻花三

年才開發完成並於 2002 年 2 月問世其中由於產品的改版並且受限制的應用程

式( managed application )及 XML Web 服務之相關技術都包含在這一版本Visual

StudioNET 緊接於NET server 2003 之後上市而二者皆以NET Framework 21 為

主體架構

針對微軟的開發工具而言新一代的開發工具似乎是代表一種新技術的誕生

從 1993 年 C++開發工具 Microsoft C++ 70 開始同時期又有 Visual C++10 出現

由於 C++ 70 本身以比前一版開發工具改良許多所以許多人並不想升級使用

Visual C++來開發工具程式直至 Visual C++推出完整視窗版的開發環境其中包含

一整合型編輯器除錯工具原始碼之瀏覽等強大開發工具大家才改用新版的

Visual C++ 來開發程式

同時由於導入微軟功能類別程式庫(MFC20)也增加該產品的普及性當使

用者開發視窗程式不必切換 DOS 介面即可直接在視窗程式下編譯程式

經過九年的演進Visual StudioNET可提供快速並且有效的建置檔案軟體元件

或是以 XML 為溝通語言的 Web 服務由 1998 年第一個版本 Visual Studio裡面包

含 Visual C++Visual Basic Visual CVisual InterDev 及 Visual J++等產品為開

發應用程式的最佳開發工具Visual Studio 6能達成上敘的所有功能它涵蓋了Win32

MFCCOMActiveATLJavaDirectX 和 Visual Studio 6 中建置出來

NET 的其中一個目的在於簡化一般應用程式以及分散 Internet 環境中應用程

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

38

式的開發提供開發分散式系統的開發者能很快的建置所開發的軟體元件到不同的

機器上並能跨至網路的連節的建置NET 提供一種執行時間的共同語言( common

Language RuntimeCLR )可訊速的建置好不同程式語言開發軟體元件執行時期的

共同語言透過NET Framework 的類別程式庫來達成這項功能圖 1-1 為執行時期的

共同語言與NET Framework 類別程式庫的關聯性

圖 4-11 NET Framework 類別程式庫和 CLR 是NET Framework 中二項主要元件

在許多跨平台的場合中Visual BasicCC++和 COBOL 程式設計師是被隔離

開的他們聚在一起為了解決功能上互通的問題其最終的結果通常是將個別的

開發過程口頭上的互相溝通協調因此受限的 CLR 環境提供了一些重要好處可

避免因版本不同而不能相容的問題(如DLLhellip等)因此此平台所開發的應用程式

不需擔心執行上的問題同時可以利用應用程式介面來建置橫跨 Windows 及 Web

架構的應用程式

在使用 CLR 的場合中開發工作變的更具有操控性相同的NET Framework

類別程式庫可跨語言來執行類別程式庫中的 CTS 確保元件可以輕易在NET 程式

語言中共享(圖 4-8)並且在其中能安全及有效率的互相溝通圖中元件甚至可以使

用 XML Web 服務的機制

NET Framework Class Library

CLR

Managed Application

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

39

compiler

圖 4-12 NET 允許不同程式語言於使用受限的環境

CLR 提供了現今主要程式語言所使用的目標環境Visual Studio NET 支援了

Visual C++Visual Basic NETVisual J NET 和 Visual C可將協力廠商不斷開

發出來的程式語言加入 Visual Studio NET 的支援包含了 COBOLEiffelPython

和 Perl

新的編譯器選項

程式碼最佳化 ( Code OptimizationGL )G 切換控制在專案上執行整個程

式的最佳化這最佳化的動作通常可以得到足以量測的效能改善需先行閱讀此切

換控制說明文件的注意選項

緩衝區溢滿檢查 ( Buffer Overrun ChecksGS )安全程式碼的運作是藉由

在函式的回傳位址之前的堆疊上分配一些記憶體當函式進入時cookie 也跟著置

入該區函式離開時則會檢查 cookie 的安全狀態當變更過則會啟動安全錯誤

處理順序(預設值使用者會被通知並跳離該程序可以使用_set_security_handler 函

式來變更預設的錯誤處理程序)

執行期錯誤檢查 ( Run-time error checksRTCn )執行期錯誤檢查的預設

主要是協助您截取正常情況下難以偵測的錯誤RTcn 切換控制幫助您偵錯堆疊破壞

(RTCs)針對未啟始化的相依性(RTCu)以及當您指定較大的資料給較小的變數時

Visual C Web Forms Application

MSIL Native Code

Visual Basic NET Class Library

MSIL Native Code

Managed Visual C++ Class Library

MSILamp Native Code Native Code

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

40

所造成的資廖移失和陣列溢滿(RTCc)

RTCn 和GS 切換控制一樣執行期錯誤的檢查在編譯時期注入程式碼RTCn

切換控制並非以最佳化的o 建置而當您嘗試以最佳化執行時會得到錯誤的結果

RTCn 切換控制已經整合入 Visual Studio NET 的偵錯器中如果有設定執行期錯誤

偵測的話應用程式預設上會進入偵錯器中

設定可透過「 方案總管 」中的「 屬性頁 」對話框設定任何一種 Visual

Studio NET IDE 中的編譯切換控制在設定該專案上單擊滑鼠左鍵然後點選

「 屬性 」

Visual Studio NET IDE 的視窗分成二大類

文件視窗( Document Windows )位於 IDE 的中央區域含有編輯器設計工

具WEB 網頁或說明主題

工作視窗( Tool Windows )是 IDE 中用來呈現公用程式的視窗包含了「 方

案總管 」「 類別檢視 」視窗和「屬性 」視窗hellip等

於首次執行 Visual Studio NET 時會在「啟始頁」中設定「我的設定檔」頁面

如圖 4-10 所示設定檔為專門為特定程式設計視窗大小鍵盤和說明配置能夠選

則適合的類型並應用這些 IDE 的特性此頁為協助編排視窗和鍵盤的配置以便

於將 Visual Studio 6 轉換為 Visual Studio NET 的開發環境

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

41

讀取 轉換

圖 4-13 於「啟始頁」中設定偏好設定值

本論文則是使用 Robix 公司所提供的 Source Code 來驅動伺服馬達其中的檔

案為DLL 檔如果要使用驅動程式驅動則需要透過此頁面自動轉換為 Visual

Studio NET 可呼叫之元件庫

置入cpp 檔

圖 4-14 dll 檔轉換設定示意圖

Visual Studio NET Rascal Dllbas cpp 檔

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

42

No

431 程式軟體架構流程

圖 4-15 軟體架構示意圖

設定 IRQ 設定值

Yes

No

讀取 Rascal Dllbas

Timer 計時器

Yes

6顆馬達定位於角度0度

執行指令指定 6顆

馬達定位於新位置

使用計時器

設定方波

各關節轉動

已到定位

No

Yes

讀取 LPT

Port

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

43

432 程式執行頁面讀取流程

背景程式說明

先定義各類名稱

Force explicit declaration of all variables

Option Explicit

Dim RetCode As Long scratch variable records a return value

Dim ConfigPath As String path to configuration file

Dim ScriptPath As String path to script file which contains macros

Dim Macro1 As String name of macro 1

Dim Macro2 As String name of macro 2

Dim SwitchNum As Long switch number to read

Dim EiHandle As Long Ei handle

Dim RobotHandle As Long Robot handle

Dim gActiveServo As Integer

前置面版 執行讀取狀態設定 執行載入動作

執行手動設定讀取字串

圖 4-16 軟體控制介面

執行讀取狀態設定設定執行開始初始化並讀取 LPT Port 是否連結正常讀取

錯誤或連結方式錯誤則出現 Error

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

44

執行載入動作設定伺服馬達設定於 0 度並執行角度於定位的位並抓取物件

執行手動設定讀取字串可設定每顆馬達於定位的位置角度與伺服馬達參數的比

例為參數設為 14 則馬達正轉一度依此類推

44 實驗結果與討論

441 機械手臂

實驗過程中擺放 2 顆木頭做的圓球利用關節上的彎曲將抓取 2 個不同位置的木

頭圓球藉以了解到機械手臂( 圖 4-17 )彎曲時所能接受的有效抓取範圍假設一

圓球的抓取範圍空間可以將一半圓球所能抓取的空間的點都能抓取如果要使得

一圓球的範圍點都能夠抓取可以提升自由度選取範圍而自由度的提升需要調整

手臂關節結構與伺服馬達的顆數增加

圖 4-17 機械手臂外觀結構圖

442 機械蟲

利用串接的方式將伺服馬達連接成機械蟲的造型再配合控制器的控制將可

模擬蟲在爬行的狀態( 圖 4-18 ) 將機械蟲放置於光滑的平面上利用定位控制控

制其動作在移動時則能了解到將其機械蟲保持平衡定度以及讓它能夠慢慢向前

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

45

移動於目標位置則能了解到定位控制的穩定度

圖 4-18 機械蟲外觀結構圖

443 問題與討論

本次實驗運用 MatlabSimulinkVirsual Studio 2003Virsual Basic 60 圖控程式

及人機介面操作面板在硬體方面優點為減少除錯修改電路的負擔不需像 8051

晶片一樣需測試每一支針角的交握功能而對於不同系統的控制器也僅修改程式

即可缺點為撰寫出操作方便的介面程式並不容易但使用圖形介面軟體來控制

對於從事軟硬體行業的人相對減少設計電路的負擔與除錯方面的困擾將硬體的

各項功能交給可靠及穩定的軟體來撰寫則能減輕硬體設計的各種問題並減輕設計

者的負擔在人機介面方面在操控面板上輸入 PD 控制器所需設定的初始值

調整參數能馬上知道結果及誤差的大小PD 控制器不但能容易設計且實現加上

PD 控制器成本低比較容易實現所以非常適合一般的受控系統

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

46

第五章 結論

51 討論分析

由實驗結果及圖形觀察得知 PD 控制器能實現機械手臂的定位控制本論文在

實作方面為了減輕本身關節的重量所使用的支撐物採鋁製金屬元件如圖 4-17 所

示並且採用伺服馬達的齒輪來替代傳統連桿裝置以避免產生相互的作用力將

兩顆伺服馬達固定於鋁製金屬上再將兩顆伺服馬達的齒輪齟合此方法能減緩連

桿連結時運動產生的摩擦力再連結前置作業所組裝好的馬達就形成一支手臂

兩支手臂連結便可形成一支可彎曲的關節

由這些實作可發現伺服馬達的運用可以達到各式動作的要求以此做為基礎便

能建構出各式能模仿人類關節的動作或製做出各式生物機器人的各種動作如機

器蟲機器蜘蛛機器玩偶人形機器人hellip等各式以模仿生物的各種動作透過

直流伺服馬達作為關節的傳動能模擬出人類在走路並與地面滑行的運動的方式

以物理的運動向量作為數學模型並以人類的運動模做為基礎就能了解到設計控

制器必須要具備更高的準確性及穩定性本實作以伺服馬達作為內部零件而言

本身內部構造是透過磁場感測器來做驅動因此可以即時控制電流大小與相位轉子

的磁場和定子所產生的磁場就以零件而言本身的內部構造具有一定使用年限

不像人類具有組織修復及再造的功能使的挑選零件也是重要的一環才能方便於

往後的研究數據分析在校準伺服馬達時因為伺服馬達本身齒輪比太少以致於

需要精準的調整於預定位置才能準確紀錄機械手臂轉動的角度變化做為往後資

料整理的依據

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

47

52 未來研究方向

在文獻[2][6][7][11][12][15][16]中是將最佳控制的方法應用到機械手臂的強健

控制使得強健控制器的設計得以完成其所建構的機械手臂模型得以使用強健

控制的方法所設計的控制器在負載的情況下能達到系統穩定且強健的目標因

此在往後的研究方向可以以此方法為基礎設計一控制器來達到同樣或更好的

工作目標並進一步改善將針對特定情況的機械手臂系統設計具有強建性的控制

器[13][14]來提升系統的穩定度及強健性

本論文中運用定位控制系統實現機械手臂及機械蟲的動作藉由控制各伺服

馬達使其定位於目標及抓取物體如果在伺服馬達前端加裝感應器自動辨識路徑

及分辨物體大小及內容物增加系統的定位功能並且能自動判斷外在環境因素使

得定位功能增強如機械手臂在抓取物體時可利用前端的感應器(如熱感應器微

型攝影機hellip等)判斷物體的體積重量及光滑度控制機械手臂抓取物體所需的力

量以及抓取物體所需承載的重量皆是可考慮進去的因素如在機械蟲的前端加裝

感應器則能自我索引行走的路線是否能保持平衡使感應器偵測到的準心能自我

調整於直線上行走使機械蟲能定位於目的地上述功能上的設計皆是往後可研

究的方向與目標

53 研究心得

在籌劃製作機械手臂的時後從當初機械手臂的草圖設計製作完成初步的外

觀電子材料的挑選伺服馬達的介面卡的選擇測試訊號的傳入抓取傳輸介面

訊號完成後還需要測試伺服馬達的訊號寬度接下來程式規劃取得自己要的

模擬路線模擬訊號完成後再編寫控制伺服馬達的軟體將模擬以及控制訊號來

操控伺服馬達從這些過程中皆是一步一步的將它完成在製作的過程中有許

多困難挑戰著我更何況尚需依照時間表將進度完成其中角度上的設計材質上

的考量以及時間上的安排皆是在考慮的範圍內在這些制作的過程中感謝實

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

48

驗室提供我充足的資源

往後機械手臂可以在上面增加一些功能利用最為通行也最為實惠的網路攝

影機放置在機械手臂抓取的前端加裝攝影機來擷取影像並刻畫影像畫面的精

細度讓機械手臂的精確度提升瞄準畫面的準心提高當然畫面準心提高所需

的 CPU 時脈則需提升則現階段的高階電腦都足能應付控制器的部份則可考慮

將系統增加強健性的功能讓控制系統受到外在干擾或環境上的因素系統能做自

我修正數據調整目標位置而這些功能完成後將可應用在現在最為熱門的機器人

模型上針對於機器人於下半身不能保持平穩並且不能自我修正定位也就是平衡

的問題如果將上述的功能完成後將可解決此問題並且能自我保持平衡此技術

上的發展在未來不管在商業上的推動都將為可行性的目標

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

49

參考文獻

[1] FI Ahmed AM El-Tobshy AA Mahfouz and MMS Ibrahim ldquoP-I and I-P controllers in a closed loop for DC motor drivesrdquo Power Conversion Conference pp 613-618 1997

[2] T-S Chung ldquoAn inherent stability problem in Cartesian compliance and an

alternative structure of compliance controlrdquo IEEE Trans Robot Automat vol 7 no 1 pp 21-30 1991

[3] S J Chapman Matlab Programming for engineers BrooksCole Pub Co 2004

[4] MJ Er ldquoDigital-signal-processor based multirate control of a two-link flexible-joint

robotrdquo 1998 Proc of the 1998 IEEE International Conf Control Applications pp1210-1214 vol 2 1998

[5] BA Francis ldquoThe optimal linear quadratic time-invariant regulator with cheap

controlrdquo IEEE Trans Automat Contr vol 24 no 4 pp 616-621 1979

[6] K Furuta S Hara and S Mori ldquoA class of systems with the same observerrdquo IEEE Trans Automat Contr vol 21 no 4 pp 572-576 1976

[7] AR Galimidi and BR Barmish ldquoThe constrained Lyapunov problem and its

application to robust output feedback stabilizationrdquo IEEE Trans Automat Contr vol AC-31 no 5 pp 410-419 1986

[8] F Jabbari and WE Schmitendorf ldquoA noniterative method for design of linear

robust controllerrdquo IEEE Trans Automat Contr vol 35 no 8 pp 954-957 1990

[9] B Johnson M Young and C Skibo Inside Microsoft Visual Studio NET 2003 Microsoft Press 2003

[10] I Kaya and DP Atherton ldquoA PI-PD controller design for integrating processesrdquo

American Control Conference pp 258-262 vol 1 1999

[11] CY Kuo and S-PT Wang ldquoRobust position control of robotics manipulator in Cartesian coordinatesrdquo IEEE Trans Robot Automat vol 7 no 5 pp 653-659 1991

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

50

[12] F Lin and R D Brandt ldquoAn optimal control approach to robust control of robot manipulatorsrdquo IEEE Conf Decision Control pp 69-77 1998

[13] F Lin and AW Olbot ldquoAn LQR approach to robustcontrol of linear systems with

uncertain parametersrdquo in Proc 35th IEEE Conf Decision Control pp 4158-4163 1996

[14] F Lin and W Zhang ldquoRobust control of nonlinear systems without matching

conditionrdquo in Proc 32th IEEE Conf Decision Control pp 2572-2577 1993

[15] G Liuand and A Goldenberg ldquoUncertainty decomposition-based robust control of robot manipulatorsrdquo IEEE Trans Control Syst Tecl vol 4 pp 384-393 1996

[16] D Maruyama H Kimura F Kajimura M Koseki and N Inou ldquoFlexible herm

-etically -sealed mobile robot for narrow spaces using hydrostatic skeleton driving mechanismrdquo 2006 IEEERSJ International Conference Intelligent Robots and Systems pp 4006-4011 2006

[17] F Ozen ldquoAn optimal switched compensation controller for flexible-link

manipulatorsrdquo American Control Conference pp1804-1808 vol 3 1998 [18] IR Petersen ldquoA Riccati equation approach to the design of stabilizing controllers

and observers for a class of uncertain linear systemsrdquo IEEE Trans Automat Contr vol 30 no 9 pp 904-907 1985

[19] T Sumrall D Misaki H Aoyama A Himoto and O Fuchiw aki ldquoMicro hopping

robot with IR sensor for disaster survivor detectionrdquo 2005 IEEE International Workshop Security and Rescue Robotics Safety pp 189-194 2005

[20] T Senjyu and K Uezato ldquoAdjustable speed control of brushless DC motors without

position and speed sensorsrdquo Proc of the IEEE Conference on Industrial Automation and Control Emerging Technology Applications pp 160-164 1995

[21] J B Song D Ryu C Cho and M Kim ldquoDesign of a 6 DOF haptic master for

teleoperation of a mobile manipulatorrdquo IEEE International Conf vol 3 pp 3243-3248 2003

[22] J E Slotine and W Li Applied Nonlinear Control Prentice-Hall Inc 1991

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

51

[23] MW Spong ldquoOn the robust control of robot manipulatorsrdquo IEEE Trans Automat Contr vol 37 no 11 pp 1782-1786 1992

[24] JC Willems ldquoLeast squares stationary optimal control and the algebraic Riccati

equationrdquo IEEE Trans Automat Contr vol AC-16 no6 pp 621-634 1971

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

52

附錄 A 程式碼

Force explicit declaration of all variables Option Explicit Dim RetCode As Long scratch variable records a return value Dim ConfigPath As String path to configuration file Dim ScriptPath As String path to script file which contains macros Dim Macro1 As String name of macro 1 Dim Macro2 As String name of macro 2 Dim SwitchNum As Long switch number to read Dim EiHandle As Long Ei handle Dim RobotHandle As Long Robot handle Dim gActiveServo As Integer ----------------------------------------------------------------------- This function is executed when the button labeled Execute Immediate Script is clicked This function executes the script command contained in the main forms edit box ----------------------------------------------------------------------- Private Sub bnImmScript_Click() Dim ImmScript As String Get immediate script from edit box on main form ImmScript = ebImmScriptText Execute immediate script Execute the macro 1 time Note return code RetCode = rbxScriptImmediate(RobotHandle ImmScript) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing immediate script RetCode Exit Sub End If

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

53

At this point the immediate script is being executed by the DLL in a separate thread this program will continue running while the script is executing In this demo we do not explicitly wait for the immediate script to finish executing End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is loaded In here we specify our program variables ----------------------------------------------------------------------- Private Sub Form_Load() A Robix Rascal Configuration (rbc) file contains information about Eis and robots Configuration files are typically generated using the Robix Rascal Software (RascalGUIexe) IMPORTANT Three configuration files are supplied with this demo program VB_Robot_L1i7rbc - Ei connected to LPT1 using IRQ7 VB_Robot_L1i5rbc - Ei connected to LPT1 using IRQ5 VB_Robot_L2i5rbc - Ei connected to LPT2 using IRQ5 Please specify the appropriate configuration file below according to your hardware settings If none of the supplied configuration files match your hardware settings use the Rascal software to create and save a working configuration file and then specify that configuration file below See the Rascal help system for further discussion of hardware settings Set program variables EiHandle = 0 Ei handle RobotHandle = 0 robot handle SwitchNum = 1 switch to be read Configuration file path You may need to change this path if did not install to the default directory ConfigPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1_L1i7rbc

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

54

Script file path You may need to change this path if did not install to the default directory ScriptPath = CProgram FilesRobixRascalSample_ProjectsvbVB_Demo1VB_Demo1rbx Macro1 = swing macro 1 name Macro2 = step macro 2 name End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form becomes active In here we initialize the Rascal DLL (once only) ----------------------------------------------------------------------- Private Sub Form_Activate() Initialize the Rascal DLL We do this here instead of in Form_Load() so that an icon will be present during initialization This helps prevent users from losing any dialog messages that may be generated during DLL initialization Make sure we only initialize the DLL once (when the form first becomes active Static DoThisOnce As Boolean If DoThisOnce = False Then Verify DLL version RetCode = rbxDLLVersionVerify() If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error verifying DLL version RetCode End End If Check DLL functions RetCode = rbxDLLFunctionsCheck() If RetCode ltgt RBX_E_SUCCESS Then

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

55

DisplayError Error checking DLL functions RetCode End End If DoThisOnce = True End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the main form is unloaded In here we clear any configuration that may be loaded into the DLL ----------------------------------------------------------------------- Private Sub Form_Unload(Cancel As Integer) Clear the current configuration Note return code RetCode = rbxConfigClear() Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error clearing configuration RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Load Configuration is clicked This subroutine loads an existing Robix Rascal Configuration into the DLL displays some information about that configuration and assigns a script to an existing robot ----------------------------------------------------------------------- Private Sub bnLoadConfiguration_Click() Dim LptPort As Long LPT port to which a Robix Rascal Ei is connected Dim Irq As Long IRQ associated with the above LPT port Dim NameSize As Long size (in bytes) of robot name Dim RetSize As Long number of bytes of robot name copied by DLL Dim ServoCount As Long number of servos assigned to robot Dim AuxOutputCount As Long number of aux outputs assigned to robot

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

56

Load Configuration RetCode = rbxConfigFileLoad(ConfigPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error loading configuration file RetCode Exit Sub End If Get configuration information Now that the configuration is successfully loaded we need to acquire handles to the existing Ei(s) and robot(s) These handles will be needed for various function calls later In this demo we assume that only one Ei and only one robot exists Get handle of first Ei RetCode = rbxLptEiHandleGet(1 EiHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if Ei does not exist See RascalDLLDoctxt - rbxEiHandleGet() for further documentation If EiHandle = 0 Then DisplayError Ei does not exist Exit Sub End If Get Ei port information RetCode = rbxLptEiPortInfoGet(EiHandle LptPort Irq) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting Ei port information RetCode

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

57

Exit Sub End If Create a message with the info weve obtained lbEiInfoCaption = Ei enabled LptEi amp LptPort amp - IRQ amp Irq Get handle of first robot RetCode = rbxRobotHandleGet(1 RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot handle RetCode Exit Sub End If Make sure we have a valid handle Note that handle is zero if robot does not exist See RascalDLLDoctxt - rbxRobotHandleGet() for further documentation If RobotHandle = 0 Then DisplayError Robot does not exist Exit Sub End If Get the size of our robots name so we know how big our string will need to be in order to hold the name Note return code RetCode = rbxRobotNameSizeOf(RobotHandle NameSize) Check return code If (RetCode ltgt RBX_E_SUCCESS) Then DisplayRobixError Error getting size of robot name RetCode Exit Sub End If IMPORTANT Some Rascal DLL functions fill an application-provided string with characters such as rbxRobotNameGet() below Such functions expect a string large enough to hold all of the characters In order to dynamically create strings of appropriate size VB applications must create a string

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

58

and fill it with the required amount of characters as shown below If your application passes a string of incorrect size you may receive a null-pointer error code an access violation may occur or some other undesirable activity may occur Create string to hold our robots name Dim RobotName As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (NameSize was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity RobotName = String(NameSize 0) Get our robots name Note return code RetCode = rbxRobotNameGet(RobotHandle RobotName NameSize RetSize) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robot name RetCode Exit Sub End If Now lets see how many servos and aux outputs are assigned to our robot Get our robots servo count Note return code RetCode = rbxRobotServoCountGet(RobotHandle ServoCount) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots servo count RetCode Exit Sub End If Get our robots aux output count Note return code RetCode = rbxRobotAuxOutputCountGet(RobotHandle AuxOutputCount) Check return code

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

59

If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting robots aux output count RetCode Exit Sub End If Display robot info in labels on the main form lbRobotInfoCaption = Robot built lbRobotNameCaption = Name amp RobotName lbRobotServosCaption = Servos amp ServoCount lbRobotAuxOutsCaption = AuxOutputs amp AuxOutputCount Assign script to robot Robots are typically controlled by commands within a script In such a case the script must first be assigned to the robot This can be done in three ways 1 passing the script as a string by calling rbxRobotScriptAssign() 2 loading the script from a text file by calling rbxRobotScriptFileAssign() 3 If a script was assigned to a robot when the configuration file was saved the script will be re-assigned to the robot when the configuration file is re-loaded into the DLL by calling rbxConfigFileLoad() In this demo we load the script directly from the text file specified by ScriptPath Assign a script from a file Note return code RetCode = rbxRobotScriptFileAssign(RobotHandle ScriptPath) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error assigning script amp ScriptPath amp RetCode Exit Sub End If Display script info in labels on the main form

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

60

lbScriptInfoCaption = Script assigned to robot lbScriptNameCaption = ScriptPath Restart robot We have now built a logical robot of some servo and aux outputs by loading the config file but a logical robot could also be built piece by piece using command such as rbxEiEnable() rbxRobotCreate() rbxRobotServoAssign() and rbxRobotAuxOutputAssign() In any case after a logical robot is built the physical Eis that it uses must be initialized Failure to restart the robot before use may cause its servos to behave erratically When a robot is restarted it will quickly com to attention by sending each servo to its initial position (initpos) Restart robot Note return code RetCode = rbxRobotRestart(RobotHandle) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error restarting robot RetCode Exit Sub End If End Sub ----------------------------------------------------------------------- This subroutine is executed when the button labeled Read Switch and Run Macro is clicked This subroutine reads a switch sensor uses the switch value to determine which macro to execute executes the macro and checks to see if execution stopped due to a syntax error in the macro ----------------------------------------------------------------------- Private Sub bnReadSwitch_Click()

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

61

Read switch value Dim SwitchValue As Long switch value Read the value of the switch specified by SwitchNum See SwitchParkAndRead() for further documentation on reading switches SwitchValue = SwitchParkAndRead(EiHandle SwitchNum) If there was an error reading the switch exit this subroutine If SwitchValue = -1 Then Exit Sub End If Display switch info to labels on the main form lbSwitchInfoCaption = Switch amp SwitchNum amp read lbSwitchValueCaption = Value = amp SwitchValue Execute macro Dim Macro As String temporary macro name holder Branch If switch value was 0 (open) we will execute Macro1 If switch value was 1 (closed) we will execute Macro2 If SwitchValue = 0 Then Macro = Macro1 Else Macro = Macro2 End If Execute the macro 1 time Note that the DLL will create a separate thread to execute the macro This means that rbxScriptMacroExecute() will probably return before the macro is finished running In order to determine when the macro has finished we must monitor the Running status of the robot (see below)

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

62

IMPORTANT Setting macro iterations (parameter 3) to 0 causes the macro to run indefinitely In this demo doing so will cause the program to hang In your own programs indefinite macro executions can be stopped by calling rbxScriptHalt() Execute the macro 1 time Note return code RetCode = rbxScriptMacroExecute(RobotHandle Macro 1) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error executing macro amp Macro amp RetCode Exit Sub End If Display macro info in a label on the main form lbMacroInfoCaption = Running macro amp Macro amp Force repaint so that the above captions are refreshed before we enter our the do-while loop below Refresh Wait for completion Since the macro is being executed by the DLL in a separate thread this program will continue running while the macro is executing In this demo we will simply wait until the macro is done running before performing any other actions The only way to tell if the macro has finished is to monitor the Running status of the robot Dim Running As Long Running status of the robot Dim EndTime As Double time at which we should stop waiting Dim EiOK As Long status of the Ei Dim ErrMsg As String temporary error message holder Dim RetryCancel As Byte message box result holder This while loop sleeps for one second checks the status of the Ei

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

63

and then obtains the Running status of the robot When Running = 0 the macro is no longer running Do Wait for one second Note that Timer() is a VB function EndTime = Timer() + 1 Do While EndTime gt Timer() Do nothing Loop Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked halt macro execution Otherwise continue trying to run the macro If RetryCancel = vbCancel Then Halt macro execution Note return code RetCode = rbxScriptHalt(RobotHandle)

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

64

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode Exit Sub End If End If End If Check Running status Check Running status of the robot Note return code RetCode = rbxScriptRunning(RobotHandle Running) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking running status RetCode Exit Sub End If Loop While Running = 1 Display macro info in a label on the main form lbMacroInfoCaption = Macro amp Macro amp finished running Check for Syntax Error In this demo we expect that the macro stopped running for one of two reasons 1 The macro completely successfully 2 The macro encountered a syntax error Here we check to see if the macro stopped due to a syntax error and display a message accordingly Note that in this demo Macro2 has an intentional syntax error in it Dim SyntaxError As Long syntax error indicator Execute the macro Note return code

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

65

RetCode = rbxSyntaxErr(RobotHandle SyntaxError) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking for syntax error RetCode Exit Sub End If Check to see if a syntax error occurred If SyntaxError = 1 Then Display message that announces a synax error lbSyntaxErrInfoCaption = A syntax error occurred Else Display message that announces no synax error lbSyntaxErrInfoCaption = No syntax errors occurred End If End Sub ----------------------------------------------------------------------- This error-reporting function accepts a variable number of items and displays them in a message box along with an OK button The user must click OK to continue ----------------------------------------------------------------------- Public Sub DisplayError(ParamArray Items()) Dim X As Variant Dim Msg As String Loop through each item and concatenate it to the message string For Each X In Items Msg = Msg amp X Next X Display error message MsgBox Msg 0 Robix Error End Sub --------------------------------------------------------------------- This error-reporting function accepts a error message string and a Robix

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

66

error code The description of the Robix error code is obtained and the error message the error code and the error description are then displayed in a message box --------------------------------------------------------------------- Public Sub DisplayRobixError(ErrMsg As String ErrCode As Long) On Error GoTo FnError Dim RetCode As Long Dim DescripSiz As Long Dim RetSiz As Long Get the size of the error description so we know how big our character array will need to be in order to hold the description Note return code RetCode = rbxErrMsgSizeOf(ErrCode DescripSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting size of error description RetCode Exit Sub End If Create string to hold error description Dim ErrDescrip As String Make sure RobotName is large enough to hold the robots name by filling it with the required number of characters (DescripSiz was acquired by calling rbxRobotNameSizeOf() above) Note that any character can be used here we just use zero for simplicity ErrDescrip = String(DescripSiz 0) Get the error description Note return code RetCode = rbxErrMsgGet(ErrCode ErrDescrip DescripSiz RetSiz) If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error getting error description RetCode Exit Sub End If Display the entire error message Note that we display the Robix error code according to the guidelines discussed in RascalDLLbas

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

67

DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description amp ErrDescrip Exit Sub FnError Display the error message (description unavailable) DisplayError ErrMsg amp Chr(10) amp Robix Error Code RBX_ amp ErrCode amp Chr(10) amp _ Error Description unavailable End Sub ----------------------------------------------------------------------- This function illustrates how to obtain a fresh switch reading safely Ei switch values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the switch is not read successfully ----------------------------------------------------------------------- Public Function SwitchParkAndRead(EiHand As Long SwitchID As Long) As Long Dim FirstMod As Long initial switch modification count (mod) Dim CurrMod As Long current switch modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim SwitchValue As Long the value of the switch Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0 Read initial mod Read mod This signals DLL to obtain a fresh reading If this

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

68

function returns RBX_E_SUCCESS FirstMod will contain the requested switch mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If This while loop reads the current switch mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the switch value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis switch and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

69

DisplayRobixError Error checking status of Ei RetCode SwitchParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read switch mod If RetryCancel = vbCancel Then SwitchParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the switch mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading switch amp SwitchID amp ErrMsg = ErrMsg + Switch mod not updated within maximum timeout period

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

70

Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else SwitchParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxSwitchModGet(EiHand SwitchID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the switchs value has been refreshed Read fresh switch value If this function returns RBX_E_SUCCESS SwitchValue will contain the current switch value Note return code RetCode = rbxSwitchValueGet(EiHand SwitchID SwitchValue)

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

71

Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for switch amp SwitchID amp RetCode SwitchParkAndRead = -1 Exit Function End If Return switch value SwitchParkAndRead = SwitchValue End Function ----------------------------------------------------------------------- This function illustrates how to obtain a fresh Adc reading safely Ei Adc values are read by the DLL only on demand ie only when requested by an application The function returns -1 if the Adc is not read successfully This function is not used in this demo and is provided for informational purposes only ----------------------------------------------------------------------- Public Function AdcParkAndRead(EiHand As Long AdcID As Long) Dim FirstMod As Long initial Adc modification count (mod) Dim CurrMod As Long current Adc modification count (mod) Dim InitialTime As Double system Time when parking begins Dim TotalTime As Long total system Time (in seconds) parked Dim EiOK As Long Ei Responding indicator Dim AdcValue As Long the value of the Adc Dim ErrMsg As String temporary message variable Dim RetryCancel As Byte message box result holder Initialize time variables Note that Time() is a VB function InitialTime = Time() TotalTime = 0

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

72

Read initial mod Read mod This signals DLL to obtain a fresh reading If this function returns RBX_E_SUCCESS FirstMod will contain the requested Adc mod Note return code RetCode = rbxAdcModGet(EiHand AdcID FirstMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If This while loop reads the current Adc mod (modification count) repeatedly until the current mod differs from the first mod that was read When the two differ this indicates that the Adc value has been refreshed (modified) Also within this while loop we check to see if the Ei is responding properly and we do a check to make sure that this loop is not executed indefinitely Note to advanced programmers Readings from the Eis Adc and analog inputs can take up to about 20 milliseconds For many applications this delay is not a problem and the program can simply wait for a new reading when desired That is what we do below On the other hand for applications where several different inputs need to be read continuously the waiting time may become excessive and the programmer may need to devise appropriate techniques to maintain a continuous set of readings with little total waiting overhead This is left as an exercise to the advanced programmer Do Check Ei status Check status of Ei If this function returns RBX_E_SUCCESS EiOK

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

73

will contain the status of the Ei Note return code RetCode = rbxLptEiResponding(EiOK) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error checking status of Ei RetCode AdcParkAndRead = -1 Exit Function End If If Ei is not responding show message If EiOK = 0 Then Create message ErrMsg = Electronics Interface not responding ErrMsg = ErrMsg amp Check cables and power light Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Cancel was clicked exit this function Otherwise continue trying to read Adc mod If RetryCancel = vbCancel Then AdcParkAndRead = -1 Exit Function End If End If Check max time Calculate total time parked TotalTime = Time() - InitialTime Check for max timeout period elapsed This is done to prevent the program from hanging if the Adc mod doesnt change Note that RBX_MAX_EI_TIMEOUT is converted from milliseconds to seconds for comparison with TotalTime

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

74

If TotalTime gt RBX_MAX_EI_TIMEOUT 1000 Then Create message ErrMsg = Unknown error reading Adc amp AdcID amp ErrMsg = ErrMsg + Adc mod not updated within maximum timeout period Display message in a message box with buttons Retry and Cancel RetryCancel = MsgBox(ErrMsg vbRetryCancel Robix Error) If Retry was clicked reset InitialTime If Cancel was clicked exit this function If RetryCancel = vbRetry Then InitialTime = Time() Else AdcParkAndRead = -1 Exit Function End If End If Read current mod Now we read current mod Note return code RetCode = rbxAdcModGet(EiHand AdcID CurrMod) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading mod for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Allow other applications to process their events DoEvents Loop While CurrMod = FirstMod We get here when the current mod is different from the first mod This indicates that the Adcs value has been refreshed

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿

75

Read fresh Adc value If this function returns RBX_E_SUCCESS AdcValue will contain the current Adc value Note return code RetCode = rbxAdcValueGet(EiHand AdcID AdcValue) Check return code If RetCode ltgt RBX_E_SUCCESS Then DisplayRobixError Error reading value for Adc amp AdcID amp RetCode AdcParkAndRead = -1 Exit Function End If Return Adc value AdcParkAndRead = AdcValue End Function

• 封面
• 摘要及目錄
• 完稿