目次>5章 第2ステップ(色々な実験)> (23)サーボモータの制御> ソース

サーボモータの制御

更新日2009.2.8

以下のソースを取り込んで、アセンブル→PIC書き込み→実行して下さい。

; ========================== ここから ==============================
;	ファイル名	RC_AD_servo.asm
;	作成日		2009.1.18
;	作成者		mit.ueno
;	バージョン	1.0
;	PIC		PIC16F84A
;	クロック	20MHz
;	概要
;		簡易A/D変換を行い、PWM制御でサーボモータを制御
;		A/D変換結果などはLCDに表示する
;	使用ポート
;		RB0	RB0/INT(積分結果入力)
;		RB4	LCD DB4
;		RB5	LCD DB5
;		RB6	LCD DB6
;		RB7	LCD DB7
;
;		RA0	LCD RS	(4:Register Select)
;		RA1	LCD E	(5:Enable Signal)
;		RA3	矩形波出力 → 積分回路
;
;		RB3	サーボモータ
;
;	TMR0(25.6uS)
;		プリスケーラ	なし
;		TMR0	128(実際は138)
;		割込み周期	(1/20MHz)*4*1*128=25.6uS
;
;	LCDフォーマット
;		0123456789012345
;		A/D=xx PWM=xx
;		Max=xx dif=xx
;
	LIST	P=PIC16F84A
	INCLUDE	P16F84A.INC
	__CONFIG	_HS_OSC & _WDT_OFF & _PWRTE_ON & _CP_OFF


	CBLOCK	0Ch
	save_w
	save_st
	AD_counter		;AD変換ワーク
	AD_max			;AD変換最大値
	AD_max_diff		;0FFh - AD変換最大値
	flag			;bit0:INT割込みフラグ
	char			;LCD表示データ
	CNT50uS
	CNT1mS
	CNT5mS
	CNT15mS
	PWM_cycle
	PWM_cycle2
	PWM_width
	PWM_width_i
	PWM_width_old
	PWM_mode		;1:PWM0.5mS ,2:PWM0.5mS〜
	WORK_0			;計算一時ワーク
	ENDC

f_int	EQU	0		;INT割込みフラグ( flag[0] )
RS	EQU	00h		;LCD Register Select
E	EQU	01h		;LCD Enable
TMR0_i	EQU	d'138'		;TMR0初期値(128+10)
PWM_05_i EQU	d'20'		;PWM最初の0.5mSカウンタ

	ORG	0
	GOTO	init

; =============== 割り込み処理 (RB0/INT,TMR0) =================
	ORG	4		;割込みの入口

	MOVWF	save_w		;W-regセーブ
	SWAPF	STATUS,W
	MOVWF	save_st		;STATUSレジスタセーブ

	BTFSC	INTCON,INTF	;INT割込み?
	GOTO	int_INT		; Yes

; ==== TMR0割込み ====
int_TMR0
	BCF	INTCON,T0IF
	MOVLW	TMR0_i
	MOVWF	TMR0		;TMR0初期化

	DECFSZ	PWM_cycle,F	;PWM 1サイクル終了?
	GOTO	int_T0_10	; No
	BTFSC	PWM_cycle2,0	;512?
	GOTO	int_T0_5	; Yes
	BSF	PWM_cycle2,0
	GOTO	int_T0_10
int_T0_5		;1サイクル終了
	BCF	PWM_cycle2,0
	BSF	PORTB,3		;パルスON
	MOVLW	01h
	MOVWF	PWM_mode	;パルス幅の最初の0.5mS
	MOVLW	PWM_05_i
	MOVWF	PWM_width	;PWMパルス幅初期化(最初の0.5mS)
	GOTO	int_exit
int_T0_10
	DECFSZ	PWM_width,F	;PWMパルスON時間経過?
	GOTO	int_exit	; No
	MOVF	PWM_mode,W
	BTFSC	STATUS,Z	;PWM_mode = 0 ?
	GOTO	int_exit	; Yes
	BTFSS	PWM_mode,0	;PWM 最初の0.5mS ?
	GOTO	int_T0_20	; No
	MOVF	PWM_width_i,W
	MOVWF	PWM_width
	MOVLW	02h
	MOVWF	PWM_mode	;パルス幅の後半可変部
	GOTO	int_exit

int_T0_20
	CLRF	PWM_mode	;パルスOFF状態
	BCF	PORTB,3		;パルスOFF
	GOTO	int_exit

; ==== INT割込み ====
int_INT
	BCF	INTCON,INTF
	BSF	flag,f_int	;INT割込みフラグON

; ==== exit ====
int_exit
	SWAPF	save_st,W
	MOVWF	STATUS		;STATUSレジスタロード
	SWAPF	save_w,F
	SWAPF	save_w,W	;W-regロード
	RETFIE			;割込み許可リターン

; ============================== 初期処理 =======================
init
	CLRF	flag
	BSF	STATUS,RP0	;■バンク1に切替え
	MOVLW	0F4h		;
	MOVWF	TRISA		;RA0,1,3は出力
	MOVLW	007h		;
	MOVWF	TRISB		;RB3-7は出力
	MOVLW	0C8h		;プリスケーラなし,プルアップなし,RB0/INT立ち上がり
	MOVWF	OPTION_REG
	BCF	STATUS,RP0	;■バンク0に切替え

	CLRF	PORTA		;PORTAをクリア
	CLRF	PORTB		;PORTBをクリア

	CALL	LCD_init	;LCD初期化
	CALL	LCD_init_disp	;LCD初期表示

	BSF	INTCON,GIE

	CALL	AD_init		;初回A/D変換&最大値設定

	CALL	wait1ms

	CLRF	PWM_width
	CLRF	PWM_cycle
	CLRF	PWM_cycle2
	CLRF	PWM_mode
	CLRF	TMR0
	BSF	INTCON,T0IE

; ============================== メイン =========================
main
	CALL	AD_proc		;簡易A/D変換

	MOVLW	084h		;LCD表示位置を4桁目に
	CALL	LCD_command
	MOVF	AD_counter,W
	CALL	LCD_hex		;A/D変換結果をLCDに16進表示

	MOVLW	08Bh		;LCD表示位置を11桁目に
	CALL	LCD_command
	MOVF	PWM_width_i,W
	CALL	LCD_hex		;PWMパルス幅をLCDに16進表示

	CALL	wait15ms
;	CALL	wait50us	;コンデンサの放電完了待ち(合計200uS)

	GOTO	main

; ========================== A/D変換処理 =====================
AD_proc
	BCF	flag,f_int	;INT割込みフラグOFF
	BCF	INTCON,T0IE	;TMR0割込み禁止
	BSF	INTCON,INTE	;RB0/INT割込み許可
	CLRF	AD_counter	;AD変換結果クリア
	BSF	PORTA,3		;AD開始
AD_proc3
	INCF	AD_counter,F
	BTFSS	flag,f_int	;INT割込み(A/D変換終了)?
	GOTO	AD_proc3

	BCF	INTCON,INTE	;RB0/INT割込み禁止
	BSF	INTCON,T0IE	;TMR0割込み許可
	BCF	PORTA,3

	; A/D変換結果を最大値(0FFh)に換算し、上位6ビットを取り出す
	MOVF	AD_counter,W
	ADDWF	AD_max_diff,W	;最大値換算
	MOVWF	WORK_0
	BCF	STATUS,C	;上位6ビットと前回との平均を求める
	RRF	WORK_0,F	;
	BCF	STATUS,C	;
	RRF	WORK_0,F	;

	MOVF	WORK_0,W	;
	ADDWF	PWM_width_old,F	;前回との平均
	BCF	STATUS,C	;
	RRF	PWM_width_old,F	;

	MOVF	PWM_width_old,W
	ADDWF	PWM_width_i,F	;
	BCF	STATUS,C	;全体平均
	RRF	PWM_width_i,F	;

	MOVF	PWM_width_i,F	;
	BTFSC	STATUS,Z	;0なら1にする
	INCF	PWM_width_i,F	;

	MOVF	WORK_0,W
	MOVWF	PWM_width_old	;今回を前回へ

	RETURN


; ====================== A/D変換初回処理 =====================
AD_init
	; A/D変換の初回値(最大値)と0FFhとの差を求める
	CALL	wait15ms
	CALL	AD_proc
	MOVF	AD_counter,W
	SUBLW	0FFh
	MOVWF	AD_max_diff	;0FFh - AD_counter

	MOVLW	0C4h		;LCD表示位置を2行4桁目に
	CALL	LCD_command
	MOVF	AD_counter,W
	CALL	LCD_hex		;A/D変換最大値をLCDに16進表示

	MOVLW	0CBh		;LCD表示位置を2行11桁目に
	CALL	LCD_command
	MOVF	AD_max_diff,W
	CALL	LCD_hex		;A/D変換最大値と0FFhの差をLCD表示

	RETURN

;================= LCDに16進数で表示 ===================
LCD_hex
	MOVWF	WORK_0
	SWAPF	WORK_0,W
	ANDLW	00Fh		;上位4ビットを
	CALL	bin2hex		;16進数で
	CALL	LCD_write	;LCDに表示

	MOVF	WORK_0,W
	ANDLW	00Fh		;下位4ビットを
	CALL	bin2hex		;16進数で
	CALL	LCD_write	;LCDに表示
	RETURN

; -----------------------------------------------------
; バイナリ4ビットをLCD表示用16進数に変換
bin2hex
	ADDWF	PCL,F

	RETLW	'0'	;0
	RETLW	'1'	;1
	RETLW	'2'	;2
	RETLW	'3'	;3
	RETLW	'4'	;4
	RETLW	'5'	;5
	RETLW	'6'	;6
	RETLW	'7'	;7
	RETLW	'8'	;8
	RETLW	'9'	;9
	RETLW	'A'	;A
	RETLW	'B'	;b
	RETLW	'C'	;C
	RETLW	'D'	;d
	RETLW	'E'	;E
	RETLW	'F'	;F

;================= LCD表示をクリアする ===================
LCD_clear
	MOVLW	01h
	CALL	LCD_command
	CALL	wait5ms		;LCDのBFをチェックしないので1.64mS以上待つ
	RETURN

;================= LCDのカーソル位置を先頭に戻す =========
LCD_home
	MOVLW	080h
	CALL	LCD_command
	RETURN

;================= LCDのカーソル位置を2行目の先頭に =====
LCD_2line
	MOVLW	0C0h
	CALL	LCD_command
	RETURN

;================= LCDのディスプレイをONにする =========
LCD_on
	MOVLW	0Ch
	CALL	LCD_command
	RETURN

;================= LCDのディスプレイとカーソルをONにする ==
LCD_on_cur
	MOVLW	0Eh
	CALL	LCD_command
	RETURN

;================= LCDのディスプレイをOFFにする =======
LCD_off
	MOVLW	08h
	CALL	LCD_command
	RETURN

;================= LCDにデータを送る =====================
LCD_write
	MOVWF	char

	BSF	PORTA,RS	;RS=1(Data)

	MOVLW	00Fh		;PORTBの上位4ビットを
	ANDWF	PORTB,F		; クリア
	MOVF	char,W		;上位
	ANDLW	0F0h		;4ビットを
	IORWF	PORTB,F		;PORTB(4-7)にセット(PORTB(0-3)はそのまま)
	BSF	PORTA,E		;LCDにデータ書き込み
	NOP
	BCF	PORTA,E

	MOVLW	00Fh		;PORTBの上位4ビットを
	ANDWF	PORTB,F		; クリア
	SWAPF	char,W		;下位
	ANDLW	0F0h		;4ビットを
	IORWF	PORTB,F		;PORTB(4-7)にセット(PORTB(0-3)はそのまま)
	BSF	PORTA,E		;LCDにデータ書き込み
	NOP
	BCF	PORTA,E

	CALL	wait50us	;LCDのBFをチェックしないので50uS待つ

	RETURN

;================= LCDにコマンドを送る ===================
LCD_command
	MOVWF	char

	BCF	PORTA,RS	;RS=0(Command)

	MOVLW	00Fh		;PORTBの上位4ビットを
	ANDWF	PORTB,F		; クリア
	MOVF	char,W		;上位
	ANDLW	0F0h		;4ビットを
	IORWF	PORTB,F		;PORTB(4-7)にセット(PORTB(0-3)はそのまま)
	BSF	PORTA,E		;LCDにデータ書き込み
	NOP
	BCF	PORTA,E

	MOVLW	00Fh		;PORTBの上位4ビットを
	ANDWF	PORTB,F		; クリア
	SWAPF	char,W		;下位
	ANDLW	0F0h		;4ビットを
	IORWF	PORTB,F		;PORTB(4-7)にセット(PORTB(0-3)はそのまま)
	BSF	PORTA,E		;LCDにデータ書き込み
	NOP
	BCF	PORTA,E

	CALL	wait50us	;LCDのBFをチェックしないので50uS待つ

	RETURN

;================= LCD初期化 ================================
LCD_init
	CALL	wait5ms		;5mS待つ

	BCF	PORTA,RS	;RS=0
	BCF	PORTA,E		;E=0

	MOVLW	030h		;上位4ビットに'3'をセット
	MOVWF	PORTB
	BSF	PORTA,E		;ファンクションセット(1回目)
	NOP
	BCF	PORTA,E
	CALL	wait5ms		;5mS待つ

	BSF	PORTA,E		;ファンクションセット(2回目)
	NOP
	BCF	PORTA,E
	CALL	wait5ms		;5mS待つ

	BSF	PORTA,E		;ファンクションセット(3回目)
	NOP
	BCF	PORTA,E
	CALL	wait5ms		;5mS待つ

	MOVLW	020h		;4ビットモード
	MOVWF	PORTB		;に
	BSF	PORTA,E		;設定
	NOP
	BCF	PORTA,E
	CALL	wait1ms		;1mS待つ

	MOVLW	028h		;4ビットモード,2行表示,7ドット
	CALL	LCD_command
	CALL	LCD_off		;ディスプレイOFF
	CALL	LCD_clear	;LCDクリア
	MOVLW	06h		;
	CALL	LCD_command	;カーソルモードセット (Increment)
	CALL	LCD_on		;ディスプレイON,カーソルOFF

	RETURN


;================= LCD初期表示 ==============================
LCD_init_disp
	MOVLW	'A'
	CALL	LCD_write
	MOVLW	'/'
	CALL	LCD_write
	MOVLW	'D'
	CALL	LCD_write
	MOVLW	'='
	CALL	LCD_write

	MOVLW	087h		;LCD表示位置を7桁目に
	CALL	LCD_command
	MOVLW	'P'
	CALL	LCD_write
	MOVLW	'W'
	CALL	LCD_write
	MOVLW	'M'
	CALL	LCD_write
	MOVLW	'='
	CALL	LCD_write

	MOVLW	0C0h		;LCD表示位置を2行0桁目に
	CALL	LCD_command
	MOVLW	'M'
	CALL	LCD_write
	MOVLW	'a'
	CALL	LCD_write
	MOVLW	'x'
	CALL	LCD_write
	MOVLW	'='
	CALL	LCD_write

	MOVLW	0C7h		;LCD表示位置を2行7桁目に
	CALL	LCD_command
	MOVLW	'd'
	CALL	LCD_write
	MOVLW	'i'
	CALL	LCD_write
	MOVLW	'f'
	CALL	LCD_write
	MOVLW	'='
	CALL	LCD_write

	RETURN

;================= 15mS WAIT ================================
wait15ms
	MOVLW	d'3'
	MOVWF	CNT15mS
wait15ms_loop
	CALL	wait5ms
	DECFSZ	CNT15mS,F
	GOTO	wait15ms_loop
	RETURN

;================= 5mS WAIT =================================
wait5ms
	MOVLW	d'100'
	MOVWF	CNT5mS
wait5ms_loop
	CALL	wait50us
	DECFSZ	CNT5mS,F
	GOTO	wait5ms_loop
	RETURN

;================= 1mS WAIT =================================
wait1ms
	MOVLW	d'20'
	MOVWF	CNT1mS
wait1ms_loop
	CALL	wait50us
	DECFSZ	CNT1mS,F
	GOTO	wait1ms_loop
	RETURN

;================= 50μS WAIT ===============================
wait50us
	; 1サイクル(4クロック):0.2μS
	; 50μS=0.2μS×250サイクル

	MOVLW	d'82'		;1
	MOVWF	CNT50uS		;1
wait50us_loop
	DECFSZ	CNT50uS,F	;1
	GOTO	wait50us_loop	;2
	RETURN			;2+1

	END
; ========================== ここまで ==============================