;********************************************************************** ;DCSERVO1 2001.12.23 ;DEVICE : PIC16F873 ;CLOCK : 20MHz ;ギヤードモータのサーボ化（単純比例制御) ; PWM 入力 PORTC-2 ; A/D 入力 CH0(PORTA-0) ; DRIVER CONT 出力 PORTB-0,1,2 ;********************************************************************** list p=16F873 #include __CONFIG _CP_OFF & _WDT_OFF & _PWRTE_ON & _HS_OSC & _LVP_OFF ;基本パラメータテーブル LEVEL1 EQU 27H ;目標まで30°(A/D 27カウント) LEVEL2 EQU 13H ;目標まで15°(A/D 13カウント) LEVEL3 EQU 04H ;目標まで 5°(A/D 4カウント) LEVEL4 EQU 00H ;目標まで 0°(A/D 0カウント) WIDTH0L EQU 060H ;LEVEL0の駆動パルス下位8BIT(0.75) WIDTH0H EQU 0DBH ;LEVEL0の駆動パルス上位8BIT WIDTH1L EQU 095H ;LEVEL1の駆動パルス下位8BIT(0.5) WIDTH1H EQU 0E7H ;LEVEL1の駆動パルス上位8BIT WIDTH2L EQU 059H ;LEVEL2の駆動パルス下位8BIT(0.3) WIDTH2H EQU 0F1H ;LEVEL2の駆動パルス上位8BIT WIDTH3L EQU 01DH ;LEVEL3の駆動パルス下位8BIT(0.1) WIDTH3H EQU 0FBH ;LEVEL3の駆動パルス上位8BIT ;汎用レジスタアドレス設定 TEMP EQU 30H ;汎用一時レジスタ PDATA1 EQU 31H ;A/D変換入力データ CDATA1L EQU 32H ;PWMパルス幅下位データ/目標値 CDATA1H EQU 33H ;PWNパルス幅上位データ SDATA1 EQU 34H ;目標値 -(マイナス) 現在位置 NRDATA1 EQU 35H ;正転:0,逆転:1 ;********************************************************************** ;ベクタテーブル ;********************************************************************** ORG 00H ; processor reset vector GOTO MAIN ; go to beginning of program ORG 04H ;割り込みアドレスセット GOTO INTR ;割り込みルーチンINTRにジャンプ ;********************************************************************** ;レジスタ初期設定 ;実行時間 0.2uS x 16cycle = 3.2uS ;********************************************************************** MAIN BSF STATUS,RP0 ;メモリーバンクを１にセット MOVLW 00EH ; MOVWF ADCON1 ;CH0のみAIN REF = VDD MOVLW 001H ; MOVWF TRISA ;TRISAに01Hをセット PORT-A0だけを入力 CLRF TRISB ;TRISBをクリア PORT-Bを出力にセット MOVLW 0FFH ; MOVWF TRISC ;TRISCにFFHをセット 入力に設定 MOVLW 081H ; MOVWF OPTION_REG ;OPTION_REGに81Hをセット PORT-B PULL UPなし BSF PIE1,TMR1IE ;TMR1オーバーフロー割り込み許可 BCF STATUS,RP0 ;メモリーバンクを０にセット CLRF PORTB ;PORTBのクリア MOVLW 080H ;Wレジスタに80Hをセット MOVWF ADCON0 ;ADCON0に80Hをセット A/D CH0,Fosc/32,STOP MOVLW 30H ;Wレジスタに30Hをセット MOVWF T1CON ;T1CONレジスタ プリスケーラ1:8 STOP ;********************************************************************* ;一発目のパルス入力を見送り ;********************************************************************* ;PORTCの2bit目が1の間、ループを繰り返す。１発目のパルス見送り。 PASS1 BTFSC PORTC,2 ;PORTCの2bit目がOなら次をスキップ GOTO PASS1 ;ラベルPASS1にジャンプ ;PORTCの2bit目が0の間、ループを繰り返す。２発目のパルスを待つ。 PASS2 BTFSS PORTC,2 ;PORTCの2bit目が1なら次をスキップ GOTO PASS2 ;ラベルPASS2にジャンプ ;********************************************************************* ;3mSのタイマーを起動する。 ;実行時間 0.2u x 7cycle = 1.4uS ;********************************************************************* PROC1 ;3mS = 1875count = 0753H → FFFF - 0753H = F8ACH BCF INTCON,PEIE ;周辺割り込みの禁止 BCF INTCON,GIE ;全体割り込みの禁止 MOVLW 0ACH ; MOVWF TMR1L ;TMR1下位8ビットにDFHをセット MOVLW 0F08H ; MOVWF TMR1H ;TMR1上位8ビットにB1Hをセット BSF T1CON,TMR1ON ;TMR1のTMR1ONビットをセット カウント開始 ;*********************************************************************** ;ポテンショメータのA/D変換処理 ;実行時間（約）15u + 19.2u + (0.2u x 6cycle) = 35.4uS ;*********************************************************************** MOVLW 081H ; MOVWF ADCON0 ;ADCON0に81Hをセット A/D CH0,Fosc/32,CHARGE CALL ADWAIT ;15uSの待機 チャージ完了を待つ。 BSF ADCON0,GO ;ADCON GOビットを1にセット 変換開始 WAIT ;AD変換実行中ループする。 BTFSC ADCON0,GO ;GOビットが0なら、次の命令をジャンプ GOTO WAIT ;ラベルWAITに戻る MOVF ADRESH,W ;AD変換データを取り込む MOVWF PDATA1 ;AD変換データをPDATA1に保存 ;************************************************************************ ;PWMパルスの立下りを検出し、その時のタイマー値を読み取る ;実行時間　立下り検出後から 0.2uS x 6cycle = 1.2uS ;************************************************************************ SCANDWN BTFSC PORTC,2 ;PORTCの2bit目がOなら次をスキップ GOTO SCANDWN ;ラベルSCANDWNにジャンプ MOVF TMR1L,W ;カウンタTMR1Lを取り込む MOVWF CDATA1L ;CDATA1Lに保存 MOVF TMR1H,W ;カウンタTMR1Hを取り込む MOVWF CDATA1H ;CDATA1Hに保存 BCF T1CON,TMR1ON ;TMR1のTMR1ONビットをクリア カウント停止 ;************************************************************************ ;読み取った16ビットのパルス幅データの10〜3ビット目を取り出して ;ひとつの8ビットデータに変換する。これが回転角度の目標値となる。 ;プリスケーラ1/8(1.6μS)動作を1/32(6.4μ)動作と等価にする。 ;実行時間 0.2uS x 11cycle = 2.2uS ;************************************************************************ RRF CDATA1L,F ;CDATA1Lを1ビット右へシフト RRF CDATA1L,F ;もう1ビット右へシフト MOVLW 3FH ;Wレジスタに3FHをセット ANDWF CDATA1L,F ;CDATA1Lの上位2ビットをマスク RRF CDATA1H,F ;CDATA1Hを1ビット右へシフト RRF CDATA1H,F ;もう1ビット右へシフト RRF CDATA1H,F ;キャリービット分もう1ビット右へ MOVLW 0C0H ;WレジスタにC0Hをセット ANDWF CDATA1H,F ;CDATA1Hの上位2ビット以外をマスク MOVF CDATA1H,W ;CDATA1Hの値をWレジスタにセット IORWF CDATA1L,F ;CDATA1H or CDATA1L を CDATA1Lにセット ;************************************************************************ ;目標値CDATAL1と現在位置PDATA1を比較し、その差によって回転方向と ;モータ制御用のパルス幅を決定する。 ;実行時間　最大　0.2uS x 25cycle = 5uS ;************************************************************************ CLRF NRDATA1 ;NRDATA1のクリア MOVF PDATA1,W ;現在位置PDATA1をWレジスタへ SUBWF CDATA1L,W ;CDATA1L-PDATA1をWレジスタへ MOVWF SDATA1 ;CDATA1L-PDATA1をSDATA1へ保存 BTFSC STATUS,C ;キャリー(ボロー)が0なら次をスキップ GOTO PROCLEV4 ;PROCLEV4へジャンプ BSF NRDATA1,0 ;NRDATA1を1とする。（逆転に設定） COMF SDATA1,F ;SDATA1の各BITを反転 INCF SDATA1,F ;+1する。２の補数を作る。 ;目標値までの差が0(LEVEL4)か、0を超えるかの判定 PROCLEV4 MOVLW LEVEL4 ; SUBWF SDATA1,W ;SDATA1 - LEVEL4 を Wへ BTFSC STATUS,C ;SDATA1 - LEVEL4が-なら次をスキップ GOTO PROCLEV3 ;LEVEL3の判定にジャンプ CLRF PORTB ;PORTBをクリア　ブレーキ動作 GOTO PASS1 ;PASS1へジャンプ ;目標値までの差がLEVEL3か、LEVEL3を超えるかの判定 PROCLEV3 MOVLW LEVEL3 ; SUBWF SDATA1,W ;SDATA1-LEVEL3 BTFSC STATUS,C ;SDATA1-LEVEL3が−なら次をジャンプ GOTO PROCLEV2 ;LEVEL2の判定にジャンプ MOVLW WIDTH3L ; MOVWF TMR1L ;TMR1LにWIDTH3Lをセット MOVLW WIDTH3H ; MOVWF TMR1H ;TMR1HにWIDTH3Hをセット GOTO PROC2 ;PROC2にジャンプ ;目標値までの差がLEVEL2か、LEVEL2を超えるかの判定 PROCLEV2 MOVLW LEVEL2 ; SUBWF SDATA1,W ;SDATA1-LEVEL2 BTFSC STATUS,C ;SDATA1-LEVEL2が−なら次をジャンプ GOTO PROCLEV1 ;LEVEL1の判定にジャンプ MOVLW WIDTH2L ; MOVWF TMR1L ;TMR1LにWIDTH2Lをセット MOVLW WIDTH2H ; MOVWF TMR1H ;TMR1HにWIDTH2Hをセット GOTO PROC2 ;PROC2にジャンプ ;目標値までの差がLEVEL1か、LEVEL1を超えるかの判定 PROCLEV1 MOVLW LEVEL1 ; SUBWF SDATA1,W ;SDATA1-LEVEL1 BTFSC STATUS,C ;SDATA1-LEVEL1が−なら次をジャンプ GOTO PROCLEV0 ;LEVEL1以上の処理にジャンプ MOVLW WIDTH1L ; MOVWF TMR1L ;TMR1LにWIDTH1Lをセット MOVLW WIDTH1H ; MOVWF TMR1H ;TMR1HにWIDTH1Hをセット GOTO PROC2 ;PROC2にジャンプ ;目標値までの差がLEVEL1以上の場合の処理 PROCLEV0 MOVLW WIDTH0L ; MOVWF TMR1L ;TMR1LにWIDTH0Lをセット MOVLW WIDTH0H ; MOVWF TMR1H ;TMR1HにWIDTH0Hをセット GOTO PROC2 ;PROC2にジャンプ ;*********************************************************************** ;決定された回転方向NRDATA1とモータ制御用パルス幅TMR1L,Hをもとに ;モータに制御パルスを出力する ;実行時間　0.2uS x 9cycle = 1.8uS後にLOOP開始 ;*********************************************************************** PROC2 BTFSC NRDATA1,0 ;NRDATA1が正転なら次をスキップ GOTO REV ;逆転処理REVにジャンプ MOVLW 01H ; MOVWF PORTB ;PORTBに01Hを出力　正転動作 GOTO COUNT ;COUNTへジャンプ REV MOVLW 02H ; MOVWF PORTB ;PORTBに02Hを出力　逆転動作 COUNT BSF INTCON,PEIE ;周辺割り込みの許可 BSF INTCON,GIE ;全体割り込みの許可 BSF T1CON,TMR1ON ;TMR1のTMR1ONビットをセット カウント開始 LOOP GOTO LOOP ;TMR1カウンタFFFFまで待機 ;*********************************************************************** ;TMR1カウンタがFFFFHになった時の割り込み処理 ;モータドライブパルスの終了タイミングで発生する ;実行時間 0.2uS x 9cycle = 1.8uS ;*********************************************************************** INTR MOVLW 00H ; MOVWF PORTB ;PORTBに00Hを出力　ブレーキ動作 BCF T1CON,TMR1ON ;TMR1のTMR1ONビットをクリア カウント停止 BCF PIR1,TMR1IF ;オーバーフローフラグのクリア BSF STATUS,RP0 ;メモリーバンクを1にセット BSF PIE1,TMR1IE ;オーバーフロー割り込み許可 BCF STATUS,RP0 ;メモリーバンクを0にセット GOTO PASS1 ;PASS1にジャンプ次のPWMパルスを待つ ;*********************************************************************** ;15uSのタイマ ;20MHz 1サイクル 0.2uS ;100サイクルx0.2uS = 20uS ;*********************************************************************** ADWAIT MOVLW 020H ;1 Wレジスタに20H(32)をセット MOVWF TEMP ;1 TEMP変数に1AHをセット NOP ;1 TLOOP DECFSZ TEMP ;1x31+2 TEMP-1を行い0になったら次の命令ジャンプ GOTO TLOOP ;2x31 ラベルTLOOPに戻る RETURN ;2 戻る END