以 MSP430F149 實現單軸伺服油壓定位控制

    以 MSP430F149 實現單軸伺服油壓定位控制器            作者 : 鄭旭崇


壹.前言

    在自動控制的領域中,機構的伺服控制, 儼然已經成為機電整合的重要一環。例如:
CNC 模床,放電加工機, CNC 銑床,伺服油壓定位/定速器...等等,關於精密製造工業者,
無不是伺服控制的天下。記得在民國 88 年時,小弟曾經以 8051 加 AB 相解碼電路加 
A/D,D/A 完成一CNC伺服油壓定位器,應用在塑膠中空厚度控制,鋼心鋁線製作機器,精密磨
床...等等,都有相當令人滿意的成果。所以本篇將以伺服油壓系統為主,為各位介紹如
何實現伺服定位控制器。

    小弟這次以 TI 出品的單晶片 MSP430F149 來說明如何實現伺服定位控制,主要是因
為 MSP430F149 這一顆 MCU本身已經結合了 12 bit A/D, PWM ,16 個可規劃上下緣觸發
的外部中斷等周邊設備,著實減少了電路大小,使電路設計更為簡單,在算數運算上,有硬
體組成的加乘器,使算數運算的效率增強不少。D/A 部份可由 PWM + RC濾波 + OP 電路完
成,而 AB 相解碼電路則可由四個外部中斷輸入,做法是: A 相分別產生上緣觸發與下緣觸
發,B 相也產生上緣觸發與下緣觸發。所以總共需要: A上,A下,B上,B下,等四條中斷觸發, 
再由軟體做四倍數解碼即可。本篇文章將不會在純理論上討論太多,而是以實際經驗來描述
如何對機構的位置與速度,做最有效率的控制。因為小弟認為數學式舉太多,反而會讓讀者更
加霧煞煞。觀念的澄清,經驗的傳播才是小弟所想要的。

讀者朋友若對於文章中的內容有任何問題,歡迎來信詢問。我的 E-mail 是: finetech@ms69.hinet.net 
若想對 MSP430 進一步了解,可以到小弟的網站 www.finetech.idv.tw 上,有一些小弟自己
做的專題和技術資料,將不定期放到網路上,可供讀者參考。




貳.控制伺服機構的方法

    伺服(Servo)控制器,與一般的調節式控制(Regulate)最大的不同點在於,伺服控制器
必須能夠完全追蹤目標值,以達成隨時控制機構之位置或速度的目的。要達成此種程度的
控制,最常用的方法就是比例+積分(PI)控制法則了。

2-1 閉迴路 - 比例控制
    以伺服油壓閥控制油壓缸為例,如圖 2-1 所示,輸出為 +/-10 伏特的速度命令,而 Encoder 
檢測信號,由機構的最後端回饋回來,稱為全閉迴路控制。全閉迴路控制的優點是,不須考慮
機械本身的磨擦,間隙,等問題。精密度高。


		< 圖 2-1 >
		閉迴路 - 比例控制方塊圖		

如圖 2-1 所用的伺服驅動器接受+/-10V 的速度命令,以用來推動伺服油壓閥。比例控
制的基本原理是,將設定值減掉實際值,再乘以一個比例常數,直接輸出到伺服驅動器上。
當我們由設定端輸入一步階函數(Step function)時,假設 t>0 , S/P=0 ; t>=0,S/P=100
系統的轉移函數為 
		------ (式 2-1)

其中:
S(t) = 機構位置
t = 時間,從零到無限大
T = 時間常數 = 1/Kp

圖 2-2 是它的步階響應


		< 圖 2-2 >
		閉迴路 - 比例控制步階響應圖


    從 2-1 式子裡,可以看出當 Kp 越大時,時間常數 T 越小,也就是 Kp 越大,系統的響
應速度越快。但實際上,對數位控制器而言 Kp 大到某一個程度時,輸出的解析將會越來越
粗獷,使得系統越來越不穩定。當 Kp 由實驗結果得到一最佳值後,我們發現了一個問題,那
就是當受控物位置達穩定時,與目標值之間,存在一個小小的,不固定的誤差值,從此就不再
追蹤修正了。這種現象稱之為穩態誤差。

    穩態誤差型成的原因很多,例如:機構的磨擦力, 機構的間隙,負載的大小,伺服閥或伺
服馬達的非線性響應...等等都是造成穩態誤差的原因。因為誤差太小, 乘以比例常數輸出
到伺服驅動器上,還是無法推動機構運動。這樣機構的誤差無法得到修正, 於是穩態誤差就
此產生了。想要修正穩態誤差,必須打破誤差值與輸出之間的比例線性關係。 最直接的方法
就是把時間因素也考慮進來,將誤差值與時間積分,然後輸出,如此一來便可消除穩態誤差。


2-2 閉迴路 - 比例積分控制

    將誤差值乘上比例常數之後,再對時間積分輸出,稱為比例積分控制。圖 2-3 為加入積
分後的系統方塊圖。使用微處理機對時間積分的方法其實很簡單,只要用 Timer 設一個固定
時間中斷機制,然後在每一次中斷發生時,將積分累計變數加一。詳細方法如下:

積分累計變數 = 積分累計變數+1;
誤差值 = 設定值-實際值
積分值 = Ki*(誤差值 * 積分累計變數)
比例值 = 誤差值 * kp
輸出值 = 積分值 + 比例值


		< 圖 2-3 >
		加入積分後的系統方塊圖

    然而事實上,位置的追蹤控制,以上的方法只是理論,因為無限制的對時間積分,非常容易
使得整個系統失去控制,造成振盪,如圖 2-4 所示。


		< 圖 2-4 >
		不限制積分,所造成的振盪


    在實際的使用上,誤差值必須只在小於某一值時,才啟動積分功能。不但能保持系統的穩定
性,還能消除穩態誤差。請看圖2-5。


		< 圖 2-5>
		限制積分,系統保持穩定,還能消除穩態誤差


參.位置與速度的檢知方法

    光學尺(Encoder)信號, 為一AB相增量方式,其輸出的波形如圖3-1,與圖3-2所示,相位
相差90度,並可檢測其運動方向。利用軟體方式作判斷並加以加減計數,即可得到絕對位置,
在精密定位控制系統是非常重要的。


		< 圖3-1 >
		正向移動時,A相超前 90 度


		< 圖3-2 >
		逆向移動時,A相落後 90 度


    如何利用軟體四倍數解碼呢? 做法是: A 相分別產生上緣(rising edge)觸發與下緣
(fall edge)觸發,B相也產生上緣觸發與下緣觸發,總共需要四條中斷觸發。解碼分析如
圖3-3所示。計數器(Counter)可用內部計憶體 16 bit 即可。當中斷產生後,利用軟體流程
判斷方式解碼。這個方法不須要任何邏輯運算,不過也是最簡單,速度最快的。


		< 圖3-3 >
		AB相四倍解碼分析



MSP430 四倍解碼程式例(電路請參考圖 5-1)

PGA		equ	04H
PGB		equ	10H

Init_Port1			;初始化 P1
       	bis    	#GIE,SR		;允許 P1 中斷產生
       	bis.b  	#3cH,&P1IE      ;P1.2, P1.3, P1.4, P1.5 Interrupt Enable
	bis.b  	#28H,&P1IES     ;P1.3 P1.5 fall edge
	bic.b  	#14H,&P1IES     ;P1.2 P2.4 rising edge
       	mov.b  	#00H,&P1SEL     ;設定 P1 為輸入
	ret

PORT1_INT			;PORT1 中斷副程式
	bit.b	#04H,&P1IFG	;A up
	jz	PGAfall

	bit.b	#PGB,&P1IN
	jz	INCCounter
	jmp	DECCounter
;
PGAfall
	bit.b	#08H,P1IFG	;A down
	jz	PGBrising

	bit.b	#PGB,&P1IN
	jz	DECCounter
	jmp	INCCounter
;
PGBrising
	bit.b	#10H,P1IFG	;B up
	jz	PGBfall

	bit.b	#PGA,&P1IN
	jz	DECCounter
	jmp	INCCounter
;
PGBfall
	bit.b	#20H,P1IFG	;B down
	jz	ExitP1Int

	bit.b	#PGA,&P1IN
	jz	INCCounter
;
DECCounter
	dec	wMCounter
        dec	wSt

	mov.b	#0,&P1IFG
       	reti

INCCounter
	inc	wMCounter
        inc	wSt

	mov.b	#0,&P1IFG	;關掉 P1 interrupt pending flag 避免反覆中斷
       	reti

ExitP1Int
	mov.b	#0,&P1IFG	;關掉 P1 interrupt pending flag 避免反覆中斷

        reti



肆.梯形加減速分析

    在機構的運動中,梯形加減速可以緩衝工件或負載上的慣性,降低機械的振動,增加機構
使用壽命。所以加減速控制是非常重要的,尤其是在機構運行速度較高的場合。更加顯示加減
速的重要。 加速度,速度與位置之間的物理關係,如圖4-1所示。


		< 圖4-1 >
		梯形加減速之加速度,速度與位置之間的關係

    當我們已設定好機構所要移動的行程(距離)和進給率(Feed Rate)時,並且在某些場合
必須把加(減)速給考慮進去。此時減速度所佔用的距離,就必先被預算出來。

減速度緩衝距離算法(公式)推導:




由式 4-3 得知,只要已知初速度 Vo,和減速度 a,就可以把減速度緩衝距離利用式4-3求出來。



伍.單軸伺服油壓控制實例

5-1 功能說明

有兩個按鈕PB1與PB2。
當 PB1 按下時,軸以固定之速度歸零(Home Position)。
當 PB2 按下時,軸以梯形加速->定速前進->減至零速->目的位置(Target Position)。


5-2 電路與說明

    圖 5-1 為 MSP430F149 單軸伺服油壓定控制之電路圖,拜 SOC 所賜,使得電路變得非
常簡單。P4.1 為 PWM 輸出,經過 RC 濾波後,第一級OP(LM324)箝位(Offset)-1.25V,第二
級OP(LM324)放大8倍,即可得到 +/- 10V 的輸出電壓。箝位(Offset)電壓由調整 VR1 所得。

    光學尺的AB相輸入,經過 74HC14 舒密特反向器整型後,即可得到漂亮的方波。P1.0為機構
原點輸入。因為當系統剛開機時,無法確認目前軸的位置,所以必須有歸回原點的動作。故當P1.0
被觸發時,代表機構已達原點,此時必須立刻將 Counter 記憶體清為零。


	< 圖5-1 > MSP430F149 單軸伺服油壓定控制-電路圖


5-3 軟體流程圖

    圖 5-2 是程式總流程圖,圖 5-3 是梯形加減速流程圖。養成畫流程圖的習慣,將會有意
想不到的收獲。

    基於職場道德,小弟只發表部分流程圖,不公布程式的原始碼(Source code)。在流程圖
中的演算法與如何控制程式流程的觀念,讀者看得懂就是您自己的了。



	< 圖5-2 > 程式總流程圖 




	< 圖5-3 > 梯形加減速流程圖

    此流程圖中,變數 TRACK 就是油壓缸追蹤的目標值。TRACK 與光學尺實際位置 wMCounter
做比例積分(PI)運算後,由 P4.1 製造出 PWM 波型,經 OP 轉成 +/-10V 輸出以推動伺服油
壓放大器(Driver)。



陸.結論
    本篇文章雖名為 MSP430F149 實現單軸伺服油壓定位控制器,但是在伺服控制理論與
實際講解上所花的篇幅較多,反而在說明如何使用 MSP430 的篇幅上比較少。小弟會選用
MSP430F149 的原因是因為可以簡化大部分的線路,而且 SOC(system On Chip)  晶片的盛
行,也將是大勢所趨。其實我是認為,只要 performance 能力夠,用什麼 CPU 倒不是重點。
重點是要了解伺服控制的精神。讀者朋友若對於文章中的內容有任何問題,歡迎來信詢問。
我的 E-mail 是: finetech@ms69.hinet.net 若想對 MSP430 進一步了解,可以到小弟的
網站 www.finetech.idv.tw 上,有一些小弟自己做的專題和技術資料,將不定期放到網路
上,可供讀者參考。



晴天科技工作室