;********************************************************************** ;DCSERVO2 2002.02.24 ;DEVICE : PIC16F873 ;CLOCK : 20MHz ;ギヤードモータのサーボ化（単純比例制御) ４ch対応版 ;４chのPWM入力信号の立ち上がりタイミングが同期することが条件 ; ; PWM 入力 PORTC-0,1,2,3 ; A/D 入力 CH0,1,2,3(PORTA-0,1,2,3) ; DRIVER CONT 出力 PORTB-0,1/2,3/4,5/6,7 ;********************************************************************** 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カウント) OFFSET EQU 0DAH ;モータ駆動パルス発生基準初期値 WIDTH0H EQU 0FFH ;LEVEL0の駆動パルス上位8BIT(0.75) WIDTH1H EQU 0F3H ;LEVEL1の駆動パルス上位8BIT(0.5) WIDTH2H EQU 0E9H ;LEVEL2の駆動パルス上位8BIT(0.3) WIDTH3H EQU 0DFH ;LEVEL3の駆動パルス上位8BIT(0.1) ;各下位8BITは00Hとする。 ;汎用レジスタアドレス設定 TEMP EQU 20H ;汎用一時レジスタ TEMPL EQU 21H ;汎用一時レジスタ TEMPH EQU 22H ;汎用一時レジスタ TEMPS EQU 23H ;汎用一時レジスタ INTFLG EQU 24H ;割り込み動作フラグ 0:入 1:出 CAPFLG EQU 25H ;PWM幅取得フラグ3〜0bit 0:未 1:取得 ;CH1 PDATA1 EQU 31H ;A/D変換入力データ CDATA1L EQU 32H ;PWMパルス幅下位データ/目標値 CDATA1H EQU 33H ;PWNパルス幅上位データ SDATA1 EQU 34H ;目標値 -(マイナス) 現在位置 NRDATA1 EQU 35H ;正転:0,逆転:1 PWIDTH1H EQU 36H ;モータドライブパルス幅データ上位 ;CH2 PDATA2 EQU 37H ;A/D変換入力データ CDATA2L EQU 38H ;PWMパルス幅下位データ/目標値 CDATA2H EQU 39H ;PWNパルス幅上位データ SDATA2 EQU 3AH ;目標値 -(マイナス) 現在位置 NRDATA2 EQU 3BH ;正転:0,逆転:1 PWIDTH2H EQU 3CH ;モータドライブパルス幅データ上位 ;CH3 PDATA3 EQU 3DH ;A/D変換入力データ CDATA3L EQU 3EH ;PWMパルス幅下位データ/目標値 CDATA3H EQU 3FH ;PWNパルス幅上位データ SDATA3 EQU 40H ;目標値 -(マイナス) 現在位置 NRDATA3 EQU 41H ;正転:0,逆転:1 PWIDTH3H EQU 42H ;モータドライブパルス幅データ上位 ;CH4 PDATA4 EQU 43H ;A/D変換入力データ CDATA4L EQU 44H ;PWMパルス幅下位データ/目標値 CDATA4H EQU 45H ;PWNパルス幅上位データ SDATA4 EQU 46H ;目標値 -(マイナス) 現在位置 NRDATA4 EQU 47H ;正転:0,逆転:1 PWIDTH4H EQU 48H ;モータドライブパルス幅データ上位 ;********************************************************************** ;ベクタテーブル ;********************************************************************** ORG 00H ; processor reset vector GOTO MAIN ; go to beginning of program ORG 04H ;割り込みアドレスセット GOTO INTR ;割り込みルーチンINTRにジャンプ ;********************************************************************** ;レジスタ初期設定 ;実行時間 0.2uS x 15cycle = 3.2uS ;********************************************************************** MAIN BSF STATUS,RP0 ;メモリーバンクを１にセット MOVLW 002H ; MOVWF ADCON1 ;CH0-4をAIN REF = VDD MOVLW 00FH ; MOVWF TRISA ;TRISAに0FHをセット PORT-A0,1,2,3入力 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 ;メモリーバンクを０にセット MOVLW 30H ;Wレジスタに30Hをセット MOVWF T1CON ;T1CONレジスタ プリスケーラ1:8 STOP CLRF PORTB ;PORTBのクリア ;********************************************************************* ;一発目のパルス入力を見送り ;パルスの監視はPWM CH0に固定する。 ;********************************************************************* ;PORTCの0bit目が1の間、ループを繰り返す。１発目のパルス見送り。 PASS1 BTFSC PORTC,0 ;PORTCの0bit目がOなら次をスキップ GOTO PASS1 ;ラベルPASS1にジャンプ ;PORTCの0bit目が0の間、ループを繰り返す。２発目のパルスを待つ。 PASS2 BTFSS PORTC,0 ;PORTCの0bit目が1なら次をスキップ GOTO PASS2 ;ラベルPASS2にジャンプ ;********************************************************************* ;2mSのタイマーを起動する。 ;実行時間 0.2u x 7cycle = 1.8uS ;********************************************************************* PROC1 ;2mS = 1250count = 04E2H → FFFF - 04E2H = FB1DH BCF INTCON,PEIE ;周辺割り込みの禁止 BCF INTCON,GIE ;全体 割り込みの禁止 MOVLW 01DH ; MOVWF TMR1L ;TMR1下位8ビットに1DHをセット MOVLW 0FBH ; MOVWF TMR1H ;TMR1上位8ビットにFBHをセット BSF T1CON,TMR1ON ;TMR1のTMR1ONビットをセット カウント開始 ;*********************************************************************** ;ポテンショメータのA/D変換処理 ;実行時間（約）(15u + 19.2u + (0.2u x 7cycle) = 35.6uS) x4 ;*********************************************************************** ;AN0の変換 MOVLW 081H ; MOVWF ADCON0 ;ADCON0に81Hをセット A/D CH0,Fosc/32,CHARGE CALL ADWAIT ;15uSの待機 チャージ完了を待つ。 BSF ADCON0,GO ;ADCON GOビットを1にセット 変換開始 WAIT1 ;AD変換実行中ループする。 BTFSC ADCON0,GO ;GOビットが0なら、次の命令をジャンプ GOTO WAIT1 ;ラベルWAIT1に戻る MOVF ADRESH,W ;AD変換データを取り込む MOVWF PDATA1 ;AD変換データをPDATA1に保存 ;AN1の変換 MOVLW 089H ; MOVWF ADCON0 ;ADCON0に89Hをセット A/D CH1,Fosc/32,CHARGE CALL ADWAIT ;15uSの待機 チャージ完了を待つ。 BSF ADCON0,GO ;ADCON GOビットを1にセット 変換開始 WAIT2 ;AD変換実行中ループする。 BTFSC ADCON0,GO ;GOビットが0なら、次の命令をジャンプ GOTO WAIT2 ;ラベルWAIT2に戻る MOVF ADRESH,W ;AD変換データを取り込む MOVWF PDATA2 ;AD変換データをPDATA2に保存 ;AN2の変換 MOVLW 091H ; MOVWF ADCON0 ;ADCON0に91Hをセット A/D CH2,Fosc/32,CHARGE CALL ADWAIT ;15uSの待機 チャージ完了を待つ。 BSF ADCON0,GO ;ADCON GOビットを1にセット 変換開始 WAIT3 ;AD変換実行中ループする。 BTFSC ADCON0,GO ;GOビットが0なら、次の命令をジャンプ GOTO WAIT3 ;ラベルWAIT3に戻る MOVF ADRESH,W ;AD変換データを取り込む MOVWF PDATA3 ;AD変換データをPDATA3に保存 ;AN3の変換 MOVLW 099H ; MOVWF ADCON0 ;ADCON0に99Hをセット A/D CH3,Fosc/32,CHARGE CALL ADWAIT ;15uSの待機 チャージ完了を待つ。 BSF ADCON0,GO ;ADCON GOビットを1にセット 変換開始 WAIT4 ;AD変換実行中ループする。 BTFSC ADCON0,GO ;GOビットが0なら、次の命令をジャンプ GOTO WAIT4 ;ラベルWAIT4に戻る MOVF ADRESH,W ;AD変換データを取り込む MOVWF PDATA4 ;AD変換データをPDATA4に保存 BSF INTCON,PEIE ;周辺割り込みの許可 BSF INTCON,GIE ;全体割り込みの許可 CLRF INTFLG ;割り込み動作フラグをクリア CLRF CAPFLG ;PWM幅取り込みフラグをクリア ;************************************************************************ ;PWMパルスの立下りを検出し、その時のタイマー値を読み取る ;TMR1に割り込みが発生するまでループする。 ;実行時間　立下り検出後から最大 (0.2uS x 12cycle = 2.4uS)x4 ;************************************************************************ SCANDWN BTFSS PORTC,0 ;PORTCの0bit目が1なら次をスキップ CALL CAP0 ;サブルーチンCAP0の実行 BTFSS PORTC,1 ;PORTCの1bit目が1なら次をスキップ CALL CAP1 ;サブルーチンCAP1の実行 BTFSS PORTC,2 ;PORTCの2bit目が1なら次をスキップ CALL CAP2 ;サブルーチンCAP2の実行 BTFSS PORTC,3 ;PORTCの3bit目が1なら次をスキップ CALL CAP3 ;サブルーチンCAP3の実行 GOTO SCANDWN ;ラベルSCANDWNにジャンプ ;PWM0のパルス幅取り込み CAP0 BTFSC CAPFLG,0 ;CAPFLGの0bit目が0なら次をスキップ RETURN ;1ならRETURNする MOVF TMR1L,W ;カウンタTMR1Lを取り込む MOVWF CDATA1L ;CDATA1Lに保存 MOVF TMR1H,W ;カウンタTMR1Hを取り込む MOVWF CDATA1H ;CDATA1Hに保存 BSF CAPFLG,0 ;CAPFLGの0bit目に1を立てる（取得完了） RETURN ;戻る ;PWM1のパルス幅取り込み CAP1 BTFSC CAPFLG,1 ;CAPFLGの1bit目が0なら次をスキップ RETURN ;1ならRETURNする MOVF TMR1L,W ;カウンタTMR1Lを取り込む MOVWF CDATA2L ;CDATA2Lに保存 MOVF TMR1H,W ;カウンタTMR1Hを取り込む MOVWF CDATA2H ;CDATA2Hに保存 BSF CAPFLG,1 ;CAPFLGの1bit目に1を立てる（取得完了） RETURN ;戻る ;PWM2のパルス幅取り込み CAP2 BTFSC CAPFLG,2 ;CAPFLGの2bit目が0なら次をスキップ RETURN ;1ならRETURNする MOVF TMR1L,W ;カウンタTMR1Lを取り込む MOVWF CDATA3L ;CDATA3Lに保存 MOVF TMR1H,W ;カウンタTMR1Hを取り込む MOVWF CDATA3H ;CDATA3Hに保存 BSF CAPFLG,2 ;CAPFLGの2bit目に1を立てる（取得完了） RETURN ;戻る ;PWM3のパルス幅取り込み CAP3 BTFSC CAPFLG,3 ;CAPFLGの3bit目が0なら次をスキップ RETURN ;1ならRETURNする MOVF TMR1L,W ;カウンタTMR1Lを取り込む MOVWF CDATA4L ;CDATA4Lに保存 MOVF TMR1H,W ;カウンタTMR1Hを取り込む MOVWF CDATA4H ;CDATA4Hに保存 BSF CAPFLG,3 ;CAPFLGの3bit目に1を立てる（取得完了） RETURN ;戻る ;************************************************************************ ;読み取った16ビットのパルス幅データの10〜3ビット目を取り出して ;ひとつの8ビットデータに変換する。これが回転角度の目標値となる。 ;プリスケーラ1/8(1.6μS)動作を1/32(6.4μ)動作と等価にする。 ;実行時間 最大(0.2uS x 16cycle = 3.2uS)x4 ;************************************************************************ DATASFT BTFSC CAPFLG,0 ;CAPFLGの0bit目が0なら次をスキップ CALL SFT1 ;サブルーチンSFT1を実行 BTFSC CAPFLG,1 ;CAPFLGの1bit目が0なら次をスキップ CALL SFT2 ;サブルーチンSFT2を実行 BTFSC CAPFLG,2 ;CAPFLGの2bit目が0なら次をスキップ CALL SFT3 ;サブルーチンSFT3を実行 BTFSC CAPFLG,3 ;CAPFLGの3bit目が0なら次をスキップ CALL SFT4 ;サブルーチンSFT4を実行 GOTO LEVSET1 ;LEVSET1にジャンプ ;CDATA1L/Hの8bit化 SFT1 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にセット RETURN ;戻る ;CDATA2L/Hの8bit化 SFT2 RRF CDATA2L,F ;CDATA2Lを1ビット右へシフト RRF CDATA2L,F ;もう1ビット右へシフト MOVLW 3FH ;Wレジスタに3FHをセット ANDWF CDATA2L,F ;CDATA2Lの上位2ビットをマスク RRF CDATA2H,F ;CDATA2Hを1ビット右へシフト RRF CDATA2H,F ;もう1ビット右へシフト RRF CDATA2H,F ;キャリービット分もう1ビット右へ MOVLW 0C0H ;WレジスタにC0Hをセット ANDWF CDATA2H,F ;CDATA2Hの上位2ビット以外をマスク MOVF CDATA2H,W ;CDATA2Hの値をWレジスタにセット IORWF CDATA2L,F ;CDATA2H or CDATA2L を CDATA2Lにセット RETURN ;戻る ;CDATA3L/Hの8bit化 SFT3 RRF CDATA3L,F ;CDATA3Lを1ビット右へシフト RRF CDATA3L,F ;もう1ビット右へシフト MOVLW 3FH ;Wレジスタに3FHをセット ANDWF CDATA3L,F ;CDATA3Lの上位2ビットをマスク RRF CDATA3H,F ;CDATA3Hを1ビット右へシフト RRF CDATA3H,F ;もう1ビット右へシフト RRF CDATA3H,F ;キャリービット分もう1ビット右へ MOVLW 0C0H ;WレジスタにC0Hをセット ANDWF CDATA3H,F ;CDATA3Hの上位2ビット以外をマスク MOVF CDATA3H,W ;CDATA3Hの値をWレジスタにセット IORWF CDATA3L,F ;CDATA3H or CDATA3L を CDATA3Lにセット RETURN ;戻る ;CDATA4L/Hの8bit化 SFT4 RRF CDATA4L,F ;CDATA4Lを1ビット右へシフト RRF CDATA4L,F ;もう1ビット右へシフト MOVLW 3FH ;Wレジスタに3FHをセット ANDWF CDATA4L,F ;CDATA4Lの上位2ビットをマスク RRF CDATA4H,F ;CDATA4Hを1ビット右へシフト RRF CDATA4H,F ;もう1ビット右へシフト RRF CDATA4H,F ;キャリービット分もう1ビット右へ MOVLW 0C0H ;WレジスタにC0Hをセット ANDWF CDATA4H,F ;CDATA4Hの上位2ビット以外をマスク MOVF CDATA4H,W ;CDATA4Hの値をWレジスタにセット IORWF CDATA4L,F ;CDATA4H or CDATA4L を CDATA4Lにセット RETURN ;戻る ;************************************************************************ ;目標値CDATALと現在位置PDATAを比較し、その差によって回転方向と ;モータ制御用のパルス幅を決定する。 ;実行時間　最大(0.2uS x 32cycle = 7.8uS)x4 ;************************************************************************ ;モーター１ LEVSET1 CLRF NRDATA1 ;NRDATA1のクリア MOVF PDATA1,W ;現在位置PDATA1をWレジスタへ SUBWF CDATA1L,W ;CDATA1L-PDATA1をWレジスタへ MOVWF SDATA1 ;CDATA1L-PDATA1をSDATA1へ保存 BTFSC STATUS,C ;キャリー(ボロー)が0なら次をスキップ GOTO LEVSET11 ;LEVSET11へジャンプ BSF NRDATA1,0 ;NRDATA1を1とする。（逆転に設定） COMF SDATA1,F ;SDATA1の各BITを反転 INCF SDATA1,F ;+1する。２の補数を作る。 LEVSET11 MOVF SDATA1,W ;SDATA1をWレジスタへ MOVWF TEMPS ;SDATA1をTEMPSに格納 CALL PROCLEV4 ;サブルーチンPROCLEV4を呼び出し MOVF TEMPH,W ; MOVWF PWIDTH1H ;TEMPHをPWIDTH1Hへ格納 ;モーター２ LEVSET2 CLRF NRDATA2 ;NRDATA2のクリア MOVF PDATA2,W ;現在位置PDATA2をWレジスタへ SUBWF CDATA2L,W ;CDATA2L-PDATA2をWレジスタへ MOVWF SDATA2 ;CDATA2L-PDATA2をSDATA2へ保存 BTFSC STATUS,C ;キャリー(ボロー)が0なら次をスキップ GOTO LEVSET21 ;LEVSET21へジャンプ BSF NRDATA2,0 ;NRDATA2を1とする。（逆転に設定） COMF SDATA2,F ;SDATA2の各BITを反転 INCF SDATA2,F ;+1する。２の補数を作る。 LEVSET21 MOVF SDATA2,W ;SDATA2をWレジスタへ MOVWF TEMPS ;SDATA2をTEMPSに格納 CALL PROCLEV4 ;サブルーチンPROCLEV4を呼び出し MOVF TEMPH,W ; MOVWF PWIDTH2H ;TEMPHをPWIDTH2Hへ格納 ;モーター３ LEVSET3 CLRF NRDATA3 ;NRDATA3のクリア MOVF PDATA3,W ;現在位置PDATA3をWレジスタへ SUBWF CDATA3L,W ;CDATA3L-PDATA3をWレジスタへ MOVWF SDATA3 ;CDATA3L-PDATA3をSDATA3へ保存 BTFSC STATUS,C ;キャリー(ボロー)が0なら次をスキップ GOTO LEVSET31 ;LEVSET31へジャンプ BSF NRDATA3,0 ;NRDATA3を1とする。（逆転に設定） COMF SDATA3,F ;SDATA3の各BITを反転 INCF SDATA3,F ;+1する。２の補数を作る。 LEVSET31 MOVF SDATA3,W ;SDATA3をWレジスタへ MOVWF TEMPS ;SDATA3をTEMPSに格納 CALL PROCLEV4 ;サブルーチンPROCLEV4を呼び出し MOVF TEMPH,W ; MOVWF PWIDTH3H ;TEMPHをPWIDTH3Hへ格納 ;モーター４ LEVSET4 CLRF NRDATA4 ;NRDATA4のクリア MOVF PDATA4,W ;現在位置PDATA4をWレジスタへ SUBWF CDATA4L,W ;CDATA4L-PDATA4をWレジスタへ MOVWF SDATA4 ;CDATA4L-PDATA4をSDATA4へ保存 BTFSC STATUS,C ;キャリー(ボロー)が0なら次をスキップ GOTO LEVSET41 ;LEVSET41へジャンプ BSF NRDATA4,0 ;NRDATA4を1とする。（逆転に設定） COMF SDATA4,F ;SDATA4の各BITを反転 INCF SDATA4,F ;+1する。２の補数を作る。 LEVSET41 MOVF SDATA4,W ;SDATA4をWレジスタへ MOVWF TEMPS ;SDATA4をTEMPSに格納 CALL PROCLEV4 ;サブルーチンPROCLEV4を呼び出し MOVF TEMPH,W ; MOVWF PWIDTH4H ;TEMPHをPWIDTH4Hへ格納 GOTO PROC2 ;PROC2へジャンプ ;目標値までの差TEMPSが0(LEVEL4)か、0を超えるかの判定 PROCLEV4 MOVLW LEVEL4 ; SUBWF TEMPS,W ;TEMPS - LEVEL4 を Wへ BTFSC STATUS,C ;TEMPS - LEVEL4が-なら次をスキップ GOTO PROCLEV3 ;LEVEL3の判定にジャンプ MOVLW OFFSET ; MOVWF TEMPH ;TEMPHにOFFSET値をセット RETURN ;戻る ;目標値までの差TEMPSがLEVEL3か、LEVEL3を超えるかの判定 PROCLEV3 MOVLW LEVEL3 ; SUBWF TEMPS,W ;TEMPS-LEVEL3 BTFSC STATUS,C ;TEMPS-LEVEL3が−なら次をスキップ GOTO PROCLEV2 ;LEVEL2の判定にジャンプ MOVLW WIDTH3H ; MOVWF TEMPH ;TEMPHにWIDTH3Hをセット RETURN ;戻る ;目標値までの差TEMPSがLEVEL2か、LEVEL2を超えるかの判定 PROCLEV2 MOVLW LEVEL2 ; SUBWF TEMPS,W ;TEMPS-LEVEL2 BTFSC STATUS,C ;TEMPS-LEVEL2が−なら次をジャンプ GOTO PROCLEV1 ;LEVEL1の判定にジャンプ MOVLW WIDTH2H ; MOVWF TEMPH ;TEMPHにWIDTH2Hをセット RETURN ;戻る ;目標値までの差TEMPSがLEVEL1か、LEVEL1を超えるかの判定 PROCLEV1 MOVLW LEVEL1 ; SUBWF TEMPS,W ;TEMPS-LEVEL1 BTFSC STATUS,C ;TEMPS-LEVEL1が−なら次をジャンプ GOTO PROCLEV0 ;LEVEL1以上の処理にジャンプ MOVLW WIDTH1H ; MOVWF TEMPH ;TEMPHにWIDTH1Hをセット RETURN ;戻る ;目標値までの差TEMPSがLEVEL1以上の場合の処理 PROCLEV0 MOVLW WIDTH0H ; MOVWF TEMPH ;TEMPHにWIDTH0Hをセット RETURN ;戻る ;*********************************************************************** ;決定された回転方向NRDATAをもとにモータに制御パルスを出力する ;実行時間　(0.2uS x 10cycle = 2.0uS)x4 ;*********************************************************************** PROC2 BTFSC CAPFLG,0 ;CAPFLGの0bit目が0なら次をスキップ CALL MDRVUP1 ;サブルーチンMDRVUP1を実行 BTFSC CAPFLG,1 ;CAPFLGの1bit目が0なら次をスキップ CALL MDRVUP2 ;サブルーチンMDRVUP2を実行 BTFSC CAPFLG,2 ;CAPFLGの2bit目が0なら次をスキップ CALL MDRVUP3 ;サブルーチンMDRVUP3を実行 BTFSC CAPFLG,3 ;CAPFLGの3bit目が0なら次をスキップ CALL MDRVUP4 ;サブルーチンMDRVUP4を実行 GOTO COUNT1 ;COUNT1にジャンプ MDRVUP1 BTFSC NRDATA1,0 ;NRDATA1が正転なら次をスキップ GOTO REV1 ;逆転処理REV1にジャンプ BSF PORTB,0 ;PORTB 0bitに1を立てる 正転動作 BCF PORTB,1 ;PORTB,1bitをクリアする RETURN ;戻る REV1 BCF PORTB,0 ;PORTB 0bitをクリアする 逆転動作 BSF PORTB,1 ;PORTB,1bitに1を立てる RETURN ;戻る MDRVUP2 BTFSC NRDATA2,0 ;NRDATA2が正転なら次をスキップ GOTO REV2 ;逆転処理REV2にジャンプ BSF PORTB,2 ;PORTB 2bitに1を立てる 正転動作 BCF PORTB,3 ;PORTB,3bitをクリアする RETURN ;戻る REV2 BCF PORTB,2 ;PORTB 2bitをクリアする 逆転動作 BSF PORTB,3 ;PORTB,3bitに1を立てる RETURN ;戻る MDRVUP3 BTFSC NRDATA3,0 ;NRDATA3が正転なら次をスキップ GOTO REV3 ;逆転処理REV3にジャンプ BSF PORTB,4 ;PORTB 4bitに1を立てる 正転動作 BCF PORTB,5 ;PORTB,5bitをクリアする RETURN ;戻る REV3 BCF PORTB,4 ;PORTB 4bitをクリアする 逆転動作 BSF PORTB,5 ;PORTB,5bitに1を立てる RETURN ;戻る MDRVUP4 BTFSC NRDATA4,0 ;NRDATA4が正転なら次をスキップ GOTO REV4 ;逆転処理REV4にジャンプ BSF PORTB,6 ;PORTB 6bitに1を立てる 正転動作 BCF PORTB,7 ;PORTB,7bitをクリアする RETURN ;戻る REV4 BCF PORTB,6 ;PORTB 6bitをクリアする 逆転動作 BSF PORTB,7 ;PORTB,7bitに1を立てる RETURN ;戻る ;*********************************************************************** ;基準タイマーを計測し、PWIDTH*Hと一致したところで ;モータ駆動用パルスを終了させる。 ;基準タイマーの初期値はOFFSET値に設定する。 ;実行時間 0.2u x 6cycle + (0.2u x 12cycle)x3 ;*********************************************************************** COUNT1 CLRF TMR1L ;TMR1Lのクリア MOVLW OFFSET ; MOVWF TMR1H ;TMR1上位8ビットにOFFSET値をセット BSF INTCON,PEIE ;周辺割り込みの許可 BSF INTCON,GIE ;全体割り込みの許可 BSF T1CON,TMR1ON ;TMR1のTMR1ONビットをセット カウント開始 LOOP BTFSC CAPFLG,0 ;CAPFLGの0bit目が0なら次をスキップ CALL MDRVDN1 ;サブルーチンMDRVDN1を実行 BTFSC CAPFLG,1 ;CAPFLGの1bit目が0なら次をスキップ CALL MDRVDN2 ;サブルーチンMDRVDN2を実行 BTFSC CAPFLG,2 ;CAPFLGの2bit目が0なら次をスキップ CALL MDRVDN3 ;サブルーチンMDRVDN3を実行 BTFSC CAPFLG,3 ;CAPFLGの3bit目が0なら次をスキップ CALL MDRVDN4 ;サブルーチンMDRVDN4を実行 GOTO LOOP ;LOOPにジャンプ MDRVDN1 MOVF TMR1H,W ;TMR1HをWレジスタへ SUBWF PWIDTH1H,W ;PWIDTH1H - TMR1HをWレジスタへ BTFSC STATUS,C ;キャリー（ボロー）が0なら次をスキップ RETURN ;戻る BCF PORTB,0 ;PORTB 0bitをクリアする BCF PORTB,1 ;PORTB,1bitをクリアする BCF CAPFLG,0 ;CAPFLGの0bit目をクリアする RETURN ;戻る MDRVDN2 MOVF TMR1H,W ;TMR1HをWレジスタへ SUBWF PWIDTH2H,W ;PWIDTH2H - TMR1HをWレジスタへ BTFSC STATUS,C ;キャリー（ボロー）が0なら次をスキップ RETURN ;戻る BCF PORTB,2 ;PORTB 2bitをクリアする BCF PORTB,3 ;PORTB,3bitをクリアする BCF CAPFLG,1 ;CAPFLGの1bit目をクリアする RETURN ;戻る MDRVDN3 MOVF TMR1H,W ;TMR1HをWレジスタへ SUBWF PWIDTH3H,W ;PWIDTH3H - TMR1HをWレジスタへ BTFSC STATUS,C ;キャリー（ボロー）が0なら次をスキップ RETURN ;戻る BCF PORTB,4 ;PORTB 4bitをクリアする BCF PORTB,5 ;PORTB,5bitをクリアする BCF CAPFLG,2 ;CAPFLGの2bit目をクリアする RETURN ;戻る MDRVDN4 MOVF TMR1H,W ;TMR1HをWレジスタへ SUBWF PWIDTH4H,W ;PWIDTH4H - TMR1HをWレジスタへ BTFSC STATUS,C ;キャリー（ボロー）が0なら次をスキップ RETURN ;戻る BCF PORTB,6 ;PORTB 6bitをクリアする BCF PORTB,7 ;PORTB,7bitをクリアする BCF CAPFLG,3 ;CAPFLGの3bit目をクリアする RETURN ;戻る ;*********************************************************************** ;TMR1カウンタがFFFFHになった時の割り込み処理 ;INTFLG = 0の場合 → PWM幅の取得期間(2mS)が終了したら発生する。 ;INTFLG = 1の場合 → モータドライブパルス出力が終了したら発生する。 ;実行時間 INTFLG = 0 → 0.2uS x 27cycle = 5.4uS ; INTFLG = 1 → 0.2uS x 11cycle = 2.2uS ;*********************************************************************** INTR BTFSS INTFLG,0 ;INTFLGの0bit目が1の時、次スキップ GOTO CAPEND ;CAPENDにジャンプする ;モータドライバパルス出力の終了処理 CLRF PORTB ;PORTBに00Hを出力　ブレーキ動作 BCF T1CON,TMR1ON ;TMR1のTMR1ONビットをクリア カウント停止 BCF PIR1,TMR1IF ;オーバーフローフラグのクリア BSF STATUS,RP0 ;メモリーバンクを1にセット BSF PIE1,TMR1IE ;オーバーフロー割り込み許可 BCF STATUS,RP0 ;メモリーバンクを0にセット BCF INTFLG,0 ;INTFLGの0BIT目をクリア GOTO PASS1 ;PASS1にジャンプ次のPWMパルスを待つ ;PWM幅の取得期間の終了処理 CAPEND BCF T1CON,TMR1ON ;TMR1のTMR1ONビットをクリア カウント停止 BCF PIR1,TMR1IF ;オーバーフローフラグのクリア BSF STATUS,RP0 ;メモリーバンクを1にセット BSF PIE1,TMR1IE ;オーバーフロー割り込み許可 BCF STATUS,RP0 ;メモリーバンクを0にセット BSF INTFLG,0 ;INTFLGの0BIT目をセット ;規定以下のデータかを判定 CHK1 MOVF CDATA1H,F ;CDATA1Hが00Hであるか判定 BTFSS STATUS,Z ;Zフラグが1なら次をスキップ GOTO CHK2 ;CHK2へジャンプ MOVLW 02BH ;Wレジスタに2BHを格納 SUBWF CDATA1L,W ;CDATA1L - 2BHをWレジスタへ BTFSC STATUS,C ;キャリー（ボロー）が0なら次をスキップ GOTO CHK2 ;CHK2へジャンプ BCF CAPFLG,0 ;CAPFLGの0bit目をクリアする CHK2 MOVF CDATA2H,F ;CDATA2Hが00Hであるか判定 BTFSS STATUS,Z ;Zフラグが1なら次をスキップ GOTO CHK3 ;CHK3へジャンプ MOVLW 02BH ;Wレジスタに2BHを格納 SUBWF CDATA2L,W ;CDATA2L - 2BHをWレジスタへ BTFSC STATUS,C ;キャリー（ボロー）が0なら次をスキップ GOTO CHK3 ;CHK3へジャンプ BCF CAPFLG,1 ;CAPFLGの1bit目をクリアする CHK3 MOVF CDATA3H,F ;CDATA3Hが00Hであるか判定 BTFSS STATUS,Z ;Zフラグが1なら次をスキップ GOTO CHK4 ;CHK4へジャンプ MOVLW 02BH ;Wレジスタに2BHを格納 SUBWF CDATA3L,W ;CDATA3L - 2BHをWレジスタへ BTFSC STATUS,C ;キャリー（ボロー）が0なら次をスキップ GOTO CHK4 ;CHK4へジャンプ BCF CAPFLG,2 ;CAPFLGの2bit目をクリアする CHK4 MOVF CDATA4H,F ;CDATA4Hが00Hであるか判定 BTFSS STATUS,Z ;Zフラグが1なら次をスキップ GOTO DATASFT ;DATASFTへジャンプ MOVLW 02BH ;Wレジスタに2BHを格納 SUBWF CDATA4L,W ;CDATA4L - 2BHをWレジスタへ BTFSC STATUS,C ;キャリー（ボロー）が0なら次をスキップ GOTO DATASFT ;DATASFTへジャンプ BCF CAPFLG,3 ;CAPFLGの3bit目をクリアする GOTO DATASFT ;DATASFTにジャンプ データの処理を行なう ;*********************************************************************** ;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