cần giúp đỡ dspicf6010 chỉ nạp được một lần

**thanhdai** · 20-03-2009, 11:26 AM

;------------------------------------------------------------------------------

; SER VO DC 24V

; .equ __30F2010, 1
; .equ __30F2011, 1
; .equ __30F2012, 1
; .equ __30F3010, 1
; .equ __30F3011, 1
; .equ __30F3012, 1
; .equ __30F3013, 1
; .equ __30F3014, 1
; .equ __30F4011, 1
; .equ __30F4012, 1
; .equ __30F4013, 1
.equ __30F6010A, 1

.include "p30f6010A.inc"

.global __reset ;.Khai bao nhan bat dau chuong trinh (bat buoc)
;.global __T1Interrupt ;Khai bao toan cuc chuong trinh xu ly ngat Timer 1

;.global __ADCInterrupt ;Khai bao toan cuc chuong trinh xu ly ngat ADC

; .global __QEIInterrupt ;Khai bao toan cuc chuong trinh xu ly ngat ADC

; config __FOSC, CSW_FSCM_OFF & HS
; config __FOSC, CSW_FSCM_OFF & HS2_PLL8
; config __FOSC, CSW_FSCM_OFF & HS2_PLL16
; config __FOSC, CSW_FSCM_OFF & FRC
; config __FOSC, CSW_FSCM_OFF & FRC_PLL4
config __FOSC, CSW_FSCM_OFF & FRC_PLL8

;config __FOSC, CSW_FSCM_OFF & XT_PLL8
;config __FOSC, CSW_FSCM_OFF & XT_PLL16

config __FWDT, WDT_OFF
config __FBORPOR, MCLR_EN & PBOR_OFF
config __FGS, CODE_PROT_ON

;------------------------------------------------------------------------------
;Cac hang so cua chuong trinh (gia tri tuc thoi dung trong chuong trinh)

.equiv HUONGE, TRISE ;Thanh ghi trang thai cua port xuat LED
.equiv DATAE, LATE ;Thanh ghi chot cua port xuat LED
.equiv DOCE, PORTE ;Thanh ghi IN PIN

.equiv HUONGG, TRISG ;Thanh ghi trang thai cua port xuat LED
.equiv DATAG, LATG ;Thanh ghi chot cua port xuat LED
.equiv DOCG, PORTG ;Thanh ghi IN PIN

.equiv HUONGF, TRISF ;Thanh ghi trang thai cua port xuat LED
.equiv DATAF, LATF ;Thanh ghi chot cua port xuat LED
.equiv DOCF, PORTF ;Thanh ghi IN PIN

.equiv HUONGB, TRISB ;Thanh ghi trang thai cua port xuat LED
.equiv DATAB, LATB ;Thanh ghi chot cua port xuat LED
.equiv DOCB, PORTB ;Thanh ghi IN PIN

.equiv HUONGD, TRISD ;Thanh ghi trang thai cua port xuat LED
.equiv DATAD, LATD ;Thanh ghi chot cua port xuat LED
.equiv DOCD, PORTD ;Thanh ghi IN PIN

.equiv HUONGC, TRISC ;Thanh ghi trang thai cua port xuat LED
.equiv DATAD, LATD ;Thanh ghi chot cua port xuat LED
.equiv DOCC, PORTC ;Thanh ghi IN PIN

;; .equiv LED_PORT, PORTE ;LED noi vao cong E
;; .equiv LED_TRIE, TRISE ;Thanh ghi trang thai cua port xuat LED
;; .equiv LED_LAT, LATE ;Thanh ghi chot cua port xuat LED

.equ DPWM1, PDC1
.equ DPWM2, PDC2
.equ DPWM3, PDC3
.equ VITRI_EL,POSCNT
.equ TUSO,#575
.equ MAUSO,#400

;.equiv CHOT, 0

.equiv XUNG, 3
.equiv DATA, 2
.equiv XUATDATA,8

.equiv QUETLED1,15
.equiv QUETLED2,14
.equiv QUETLED3,13
.equiv QUETLED4,12 ; DD

.equiv CHOTLED1,6 ;FF
.equiv CHOTLED2,7
.equiv CHOTLED3,8
.equiv CHOTLED4,2
.equiv CHOTLED5,3

.equiv NUT4,11 ;B
.equiv NUT3,10 ;B
.equiv NUT2,9 ;B
.equiv NUT1,8 ;D

.equiv MTOTAL,0 ;B
.equiv MCOUNTER,2 ;B
.equiv MLENGHT,4 ;B
.equiv MTIMER,6 ;

.equiv MBCD1,2 ;B
.equiv MBCD2,10 ;B
.equiv MBCD3,18 ;B
.equiv MBCD4,24 ;
.equiv MBCD5,32 ;

.section .nbss,bss,near

MANGSO: .space 20
MANGBCD: .space 40

BIENMANGBCD1: .space 2
BIENMANGBCD2: .space 2
BIENMANGBCD3: .space 2
BIENMANGBCD4: .space 2
BIENMANGBCD5: .space 2

BIENNHAP: .SPACE 2
BIENSANG_TAT: .SPACE 2
BIENNHAPHIM: .SPACE 2

BCD1: .space 2
BCD2: .space 2
BCD3: .space 2
BCD4: .space 2
BCD5: .space 2

HIENTHI1: .space 2
HIENTHI2: .space 2
HIENTHI3: .space 2
HIENTHI4: .space 2

BIENMOD: .space 2
SODEM: .space 2
DICHBIT: .space 2
BIENHIENTHI: .space 2

SODEMLED: .space 2
THOIGIANLED: .space 2

BIENPHIM: .space 2
BIENHAM_DATA: .space 2
THOIGIANPHIM1: .space 2
THOIGIANPHIM2: .space 2
THOIGIANPHIM3: .space 2
LUUTHOIGIAN: .space 2
THOIGIANPHIMPR: .space 2

TOCDOTHUC: .space 2
; GIAN: .space 2
; NHAN_ENCODER: .space 2

XUNGDODUOC: .space 2
HESOTHOIGIAN: .space 2
;XUNGTRENVONG: .space 2
;THOIGIANLAYMAU: .space 2
BIENTINHTOCDO: .space 2

GOCNGHIENGTHUC: .space 2
;BIENTU: .space 2
;BIENMAU: .space 2
LUUGOCZEZO: .space 2
GOCAMDUONG: .space 2

NHAP1: .SPACE 2
NHAP2: .SPACE 2
NHAP3: .SPACE 2
NHAP4: .SPACE 2

TROLCD: .SPACE 2
TROLCD1: .SPACE 2
DEMLCD: .SPACE 2

GIUPTR: .SPACE 2
GIATRIOFSET: .SPACE 2

TFW : .SPACE 2
TRW : .SPACE 2
TDEMW: .SPACE 2
TDEMF: .SPACE 2
GIUPWMMAX:.SPACE 2

HESO_P:.SPACE 2
HESO_PP:.SPACE 2
HESO_I:.SPACE 2
HESO_D:.SPACE 2
HESO_DP:.SPACE 2

GIAN:.SPACE 2
GIAN_D:.SPACE 2
GIAN_I:.SPACE 2

HS_A:.SPACE 2
HS_B:.SPACE 2
HS_C:.SPACE 2
HS_D:.SPACE 2
KQ_H:.SPACE 2
KQ_L:.SPACE 2

BIENTU:.SPACE 2
BIENMAU:.SPACE 2

KETQUA_P:.SPACE 2
KETQUA_I:.SPACE 2
KETQUA_D:.SPACE 2
KETQUA_DH:.SPACE 2
LUUKETQUA_D:.SPACE 2

BIEN_AMDUONG:.SPACE 2

SAISO: .SPACE 2
SAISO_CU:.SPACE 2

LUUSAISO: .SPACE 2

VITRI_DATH: .SPACE 2
VITRI_DATL: .SPACE 2

VITRI_DATHCL: .SPACE 2
VITRI_DATHCH: .SPACE 2

VITRI_EH: .SPACE 2

VITRI_CNTL: .SPACE 2
VITRI_CNTH: .SPACE 2

NHAN_ENCODER: .SPACE 2

OUTPWM_MAX: .SPACE 2
OUTPWM_MIN: .SPACE 2
OUTPWM_CNT: .SPACE 2

TIMER1_OLD: .SPACE 2
TIMER2_OLD: .SPACE 2

OUTPWM: .SPACE 2
OUTPWMMAX: .SPACE 2

TIMERLOI: .SPACE 2
VANTOCLOI: .SPACE 2
ADC_DONG: .SPACE 2

THOIGIAN1: .SPACE 2
THOIGIAN2: .SPACE 2
THOIGIAN3: .SPACE 2
THOIGIAN4: .SPACE 2

THOIGIANBAOVE: .SPACE 2
THOIGIANBAOVEDONG: .SPACE 2

HIENTHILOI: .SPACE 2

BCDT1: .SPACE 2
BCDT2: .SPACE 2
BCDT3: .SPACE 2
BCDT4: .SPACE 2
BCDT5: .SPACE 2

BCDD1: .SPACE 2
BCDD2: .SPACE 2
BCDD3: .SPACE 2
BCDD4: .SPACE 2
BCDD5: .SPACE 2

BCDA1: .SPACE 2
BCDA2: .SPACE 2
BCDA3: .SPACE 2
BCDA4: .SPACE 2
BCDA5: .SPACE 2

BCDV1: .SPACE 2
BCDV2: .SPACE 2
BCDV3: .SPACE 2
BCDV4: .SPACE 2
BCDV5: .SPACE 2

;------------------------------------------------------------------------------
;Cac khai bao toan cuc:
;------------------------------------------------------------------------------

.section .const,psv
.align 256

Table_LED:
.Hword 0B1111111100001100,0B1111111110011111,0B1111111100 111000,0B1111111100011010,0B1111111110001011,0B111 1111101001010,0B1111111101001000,0B111111110001111 1,0B1111111100001000,0B1111111100001010, 0B1111111101100001,0B1111111111111111,0B1111111111 111011,0B1111111101100000,0B1111111111110001
; 0 1 2 3 4 5 6 7 8 9 10 - E R

SineTable:
.Hword 0,1,2,3,4,5,6,7,8,9,10
SineTable1:
.Hword 'T','O','C',' ','D','O',':',' ',' ',' ','0','0','0','0,'0',' ',' ','R','P','M'
.Hword 'D','O','N',' ','D','I',':',' ',' ',' ','0','0','0','0,'0',' ',' ','A','M','P'
.Hword 'V','I',' ','T','R','I',':',' ',' ',' ','0','0','0','0,'0',' ',' ',' ',' ',' '
.Hword 'D','I','E','N',' ','A',':',' ',' ',' ','0','0','0','0,'0',' ',' ','V','O','L'
SineTable3:
.Hword '0','1','2','3','4','5','6','7','8','9'

SineTable4:
.Hword 0X0A,0X0B,0X0C,0X0D,0X0E
.HWORD 0X4A,0X4B,0X4C,0X4D,0X4E
.HWORD 0X1E,0X1F,0X20,0X21,0X22
.HWORD 0X5E,0X5F,0X60,0X61,0X62

.text ;Bat dau doan ma chuong trinh

__reset:
mov #__SP_init, W15 ;Khoi tao con tro ngan xep (stack)
mov #__SPLIM_init, W0
mov W0, SPLIM ;Khoi tao thanh ghi gioi han con tro ngan xep (stack)
nop ;Can mot lenh NOP sau khi ghi vao SPLIM
clr W0 ;Xoa thanh ghi lam viec W0
mov W0, W14 ;Xoa cac thanh ghi W1 den W14
repeat #12
mov W0,[++W14] ;Dia chi cua cac thanh ghi: 0x0002 - 0x001C
clr W14

;------------------------------------------------------------------------------
rcall Init_PORTG ;Khoi tao cac cong I/O
rcall Init_PORTE ;Khoi tao cac cong I/O
rcall Init_PORTb ; init_portb
rcall Init_PORTC ; init_portC
RCALL Init_PORTD
RCALL Init_PORTF

; rcall init_adc
; RCALL init_pwm_motor

rcall UNIt_QEI
rcall Init_TMR1
rcall Init_TMR2
RCALL KHOIDONGLED

;************************************************* **************************
;************************************************* ************************************************** ******************************
;************************************************* ************************************************** ******************************

main_loop:

RCALL KIEMXUNGVAO
btss IFS2,#PWMIF ; poll the PWM interrupt flag
BRA main_loop;g
RCALL CHUONGTRINHHIENTHI
RCALL BANPHIM
bclr IFS2, #PWMIF ; Clear the PWM interrupt flag
main_loopg:

BRA main_loop

MOV VITRI_EL,W0
MOV VITRI_DATL,W2

SUB W0,W2,W4
MOV W4,SAISO
MOV W4,LUUSAISO
BRA C,CHAY_RW

CHAY_FW:

SUB W2,W0,W4
MOV W4,LUUSAISO
MOV W4,SAISO

MOV #60000,W5
SUB W4,W5,W6
BRA C,CHAY_RWW
RCALL TINH_PIDFW
BRA main_loop

CHAY_FWW:
SUB W2,W0,W4
MOV W4,SAISO
MOV W4,LUUSAISO
RCALL TINH_PIDFW
BRA main_loop

CHAY_RW:
MOV #60000,W5
SUB W4,W5,W6
BRA C,CHAY_FWW

RCALL TINH_PIDRW
BRA main_loop

CHAY_RWW:
SUB W0,W2,W4
MOV W4,SAISO
MOV W4,LUUSAISO
RCALL TINH_PIDRW
BRA main_loop
bra main_loop
;************************************************* ************************************************** ***************************
;************************************************* ************************************************** ***************************

;9999999999999999999999999999999999999999999999999 99999999999999999
CHUONGTRINHHIENTHI:

dec THOIGIANLED
bra NZ, THOATLED
MOV #10,W0
MOV W0,THOIGIANLED

BTSS SODEMLED,#0
BRA RALED2
RCALL SULILED
BCLR DATAD,#QUETLED1
RCALL BANPHIM1

RALED2:
BTSS SODEMLED,#1
BRA RALED3
INC BIENMANGBCD1
INC BIENMANGBCD2
INC BIENMANGBCD3
INC BIENMANGBCD4
INC BIENMANGBCD5
RCALL KIEMTRANHAY
RCALL SULILED
BCLR DATAD,#QUETLED2
RCALL BANPHIM2

RALED3:
BTSS SODEMLED,#2
BRA RALED4
INC BIENMANGBCD1
INC BIENMANGBCD2
INC BIENMANGBCD3
INC BIENMANGBCD4
INC BIENMANGBCD5
RCALL KIEMTRANHAY
RCALL SULILED
BCLR DATAD,#QUETLED3
RCALL BANPHIM3

RALED4:
BTSS SODEMLED,#3
BRA RALED5
INC BIENMANGBCD1
INC BIENMANGBCD2
INC BIENMANGBCD3
INC BIENMANGBCD4
INC BIENMANGBCD5
RCALL KIEMTRANHAY
RCALL SULILED
BCLR DATAD,#QUETLED4
RCALL BANPHIM4

MOV #MBCD1,W0
MOV W0,BIENMANGBCD1
MOV #MBCD2,W0
MOV W0,BIENMANGBCD2
MOV #MBCD3,W0
MOV W0,BIENMANGBCD3
MOV #MBCD4,W0
MOV W0,BIENMANGBCD4
MOV #MBCD5,W0
MOV W0,BIENMANGBCD5

DEC BIENSANG_TAT
BRA NZ,RALED5
MOV #200,W0
MOV W0,BIENSANG_TAT

RALED5:

MOV SODEMLED,W0
ADD SODEMLED

MOV SODEMLED,W0
MOV #15,W1
SUB W0,W1,W2
BRA C,THOATLED1
RETURN
THOATLED1:
MOV W2,SODEMLED

THOATLED:
RETURN

;************************************************* ***********************

SULILED:

MOV #MANGBCD,W0
ADD BIENMANGBCD1
MOV BIENMANGBCD1,W1
MOV [W1],W0
MOV #2,W1
MOV W1,NHAP1
MUL NHAP1
MOV W2,W0

RCALL LAYBANG
MOV W0,DATAD ; 8 BIT THAP
BCLR DATAF,#CHOTLED1
BSET DATAF,#CHOTLED1

MOV #MANGBCD,W0
ADD BIENMANGBCD2
MOV BIENMANGBCD2,W1
MOV [W1],W0
MOV #2,W1
MOV W1,NHAP1
MUL NHAP1
MOV W2,W0

RCALL LAYBANG
MOV W0,DATAD ; 8 BIT THAP
BCLR DATAF,#CHOTLED2
BSET DATAF,#CHOTLED2

MOV #MANGBCD,W0
ADD BIENMANGBCD3
MOV BIENMANGBCD3,W1
MOV [W1],W0
MOV #2,W1
MOV W1,NHAP1
MUL NHAP1
MOV W2,W0

RCALL LAYBANG
MOV W0,DATAD ; 8 BIT THAP
BCLR DATAF,#CHOTLED3
BSET DATAF,#CHOTLED3

MOV #MANGBCD,W0
ADD BIENMANGBCD4
MOV BIENMANGBCD4,W1
MOV [W1],W0
MOV #2,W1
MOV W1,NHAP1
MUL NHAP1
MOV W2,W0

RCALL LAYBANG
MOV W0,DATAD ; 8 BIT THAP
BCLR DATAF,#CHOTLED4
BSET DATAF,#CHOTLED4

MOV #MANGBCD,W0
ADD BIENMANGBCD5
MOV BIENMANGBCD5,W1
MOV [W1],W0
MOV #2,W1
MOV W1,NHAP1
MUL NHAP1
MOV W2,W0

RCALL LAYBANG
MOV W0,DATAD ; 8 BIT THAP
BCLR DATAF,#CHOTLED5
BSET DATAF,#CHOTLED5
RETURN

;***********************
;**************************************
LAYBANG: ; IN W0 OUT W0
MOV W0,W3
mov #tblpage(Table_LED),W0
mov W0,TBLPAG
mov #tbloffset(Table_LED),W0
ADD W0,W3,W1
TBLRDL [W1],W0
RETURN
;**********************
;0000000000000000000000000000000000000000000000000 00000000000000000
KIEMTRANHAY:

MOV BIENSANG_TAT,W0
MOV #50,W1
CPSLT W0,W1 ;; SKIP IF <
RETURN

MOV #10,W0
BTSC BIENNHAP,#0
MOV W0,BIENMANGBCD1

BTSC BIENNHAP,#1
MOV W0,BIENMANGBCD2

BTSC BIENNHAP,#2
MOV W0,BIENMANGBCD3

BTSC BIENNHAP,#3
MOV W0,BIENMANGBCD4

BTSC BIENNHAP,#4
MOV W0,BIENMANGBCD5

RETURN

;8888888888888888888888888888888888888888888888888 888888888888888
KHOIDONGLED:
MOV #0,W0
MOV W0,BIENNHAP
MOV #200,W0
MOV W0,BIENSANG_TAT ;>50 SANG

MOV #0,W0
MOV W0,BIENNHAPHIM

RETURN

;************************************************* ********************8
DIEUKHIENHIENTHILOI:
MOV HIENTHILOI,W0
RCALL TINHCHUNG
MOV #27,W0
MOV W0,HIENTHI1
MOV #29,W0
MOV W0,HIENTHI2
RETURN

;********************************************
TINHCHUNG:
;GIATRI>>W0

RCALL HEXTOBCD
MOV #2,W0
MUL BCD2 ; LAY 4 SO CUOI
MOV W2,HIENTHI1
MOV #2,W0
MUL BCD3
MOV W2,HIENTHI2
MOV #2,W0
MUL BCD4
MOV W2,HIENTHI3
MOV #2,W0
MUL BCD5
MOV W2,HIENTHI4
RETURN

;*********************************CHUONG TRINH SU DUNG LCD***************************
;-------------------------------------------------------------------
;************************************************* ************************************************** ******************************

BANPHIM1:
F1:
BTSC DOCD,#NUT1
BRA F2
BTSC BIENNHAPHIM,#0
RETURN
BSET BIENNHAPHIM,#0

F2:
BTSC DOCD,#NUT2
BRA F3
BTSC BIENNHAPHIM,#1
RETURN
BSET BIENNHAPHIM,#1

F3:
BTSC DOCD,#NUT3
BRA F4
BTSC BIENNHAPHIM,#2
RETURN
BSET BIENNHAPHIM,#2

F4:
BTSC DOCD,#NUT4
BRA FF4
BTSC BIENNHAPHIM,#3
RETURN
BSET BIENNHAPHIM,#3

FF4:

MOV #0,W0
MOV W0,BIENNHAPHIM
RETURN

;IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII IIIIIIIIIIII

BANPHIM2:

DEL:
BTSC DOCD,#NUT1
BRA P1
BTSC BIENNHAPHIM,#0
RETURN
BSET BIENNHAPHIM,#0

P1:
BTSC DOCD,#NUT2
BRA P4
BTSC BIENNHAPHIM,#1
RETURN
BSET BIENNHAPHIM,#1

P4:
BTSC DOCD,#NUT3
BRA P7
BTSC BIENNHAPHIM,#2
RETURN
BSET BIENNHAPHIM,#2

P7:
BTSC DOCD,#NUT4
BRA PP7
BTSC BIENNHAPHIM,#3
RETURN
BSET BIENNHAPHIM,#3

PP7:

MOV #0,W0
MOV W0,BIENNHAPHIM
RETURN
;999999999999999999999999999999999999999

BANPHIM3:

P0:
BTSC DOCD,#NUT1
BRA P2
BTSC BIENNHAPHIM,#0
RETURN
BSET BIENNHAPHIM,#0

P2:
BTSC DOCD,#NUT2
BRA P5
BTSC BIENNHAPHIM,#1
RETURN
BSET BIENNHAPHIM,#1

P5:
BTSC DOCD,#NUT3
BRA P8
BTSC BIENNHAPHIM,#2
RETURN
BSET BIENNHAPHIM,#2

P8:
BTSC DOCD,#NUT4
BRA PP8
BTSC BIENNHAPHIM,#3
RETURN
BSET BIENNHAPHIM,#3

PP8:

MOV #0,W0
MOV W0,BIENNHAPHIM
RETURN
;999999999999999999999999999999999999999

BANPHIM4:

OK:
BTSC DOCD,#NUT1
BRA P3
BTSC BIENNHAPHIM,#0
RETURN
BSET BIENNHAPHIM,#0

P3:
BTSC DOCD,#NUT2
BRA P6
BTSC BIENNHAPHIM,#1
RETURN
BSET BIENNHAPHIM,#1

P6:
BTSC DOCD,#NUT3
BRA P9
BTSC BIENNHAPHIM,#2
RETURN
BSET BIENNHAPHIM,#2

P9:
BTSC DOCD,#NUT4
BRA PP9
BTSC BIENNHAPHIM,#3
RETURN
BSET BIENNHAPHIM,#3

PP9:

MOV #0,W0
MOV W0,BIENNHAPHIM
RETURN
;999999999999999999999999999999999999999

KIEMTRALOI:

MOV DPWM1,W0
MOV DPWM2,W1
ADD W0,W1,W5

MOV #100,W4
repeat #17
DIV.U W5,W4 ;W5/W4=W0; REM=W1

MOV #5,W2
MOV W2,NHAP1
MUL NHAP1

DALOI:
CLR W0
MOV W0,DPWM1
MOV W0,DPWM2
BRA DALOI
RETURN
;================================================= ===

KIEMTRALOIDONG:

MOV OUTPWM,W0
MOV #40000,W1
SUB W1,W0,W2
BRA C,NOLOIW
MOV #51,W0
MOV W0,HIENTHILOI
BRA DALOIDONG

NOLOIW:
BTSS DOCE,#4
BRA LOIDONGTG
MOV #50000,W0
MOV W0,THOIGIANBAOVEDONG
CHUALOIDONG:
RETURN

LOIDONGTG:
DEC THOIGIANBAOVEDONG
BRA NZ,CHUALOIDONG
MOV #50,W0
MOV W0,HIENTHILOI

DALOIDONG:
CLR W0
MOV W0,DPWM1
MOV W0,DPWM2
BCLR PWMCON1,#PEN3H ; NGO RA CHAN PWM3H
BCLR PWMCON1,#PEN2H ; NGO RA CHAN PWM3H
BCLR PWMCON1,#PEN2L ; NGO RA CHAN PWM3H
BCLR PWMCON1,#PEN1H ; NGO RA CHAN PWM3H
BCLR PWMCON1,#PEN1L ; NGO RA CHAN PWM3H >> I/O

btss IFS2,#PWMIF ; poll the PWM interrupt flag
BRA DALOIDONG
bclr IFS2, #PWMIF ; Clear the PWM interrupt flag
RCALL DIEUKHIENHIENTHILOI
RCALL CHUONGTRINHHIENTHI

BRA DALOIDONG

RETURN

;************************************************* ***88

CHAYTHU:

DEC THOIGIAN2
BRA NZ,OUTRR
MOV #1,W0
MOV W0,THOIGIAN2

DEC THOIGIAN1
BRA NZ,OUTRR
MOV #1,W0
MOV W0,THOIGIAN1

BTSS BIEN_AMDUONG,#10
BRA NGUOC
MOV #1,W0
ADD VITRI_DATHCL ;RCALL XUNGDI
CLR W0
ADDC VITRI_DATHCH

DEC THOIGIAN3
BRA NZ,BBC
MOV #40000,W0
MOV W0,THOIGIAN3

MOV #65000,W0
MOV W0,THOIGIAN2
MOV W0,THOIGIAN1
BCLR BIEN_AMDUONG,#10
BBC:
RETURN

NGUOC:
MOV #1,W0
SUB VITRI_DATHCL
CLR W0
SUBB VITRI_DATHCH

DEC THOIGIAN3
BRA NZ,BBC
MOV #40000,W0
MOV W0,THOIGIAN3

MOV #65000,W0
MOV W0,THOIGIAN2
MOV W0,THOIGIAN1
BSET BIEN_AMDUONG,#10
BBCC:

RETURN

OUTRR:

RETURN

;************************************************* ******************

KIEMXUNGVAO:
MOV TIMER1_OLD,W1
MOV TMR1,W0
MOV W0,TIMER1_OLD
SUB W0,W1,W0
ADD VITRI_DATHCL ;RCALL XUNGDI
CLR W0
ADDC VITRI_DATHCH

MOV TIMER2_OLD,W1
MOV TMR2,W0
MOV W0,TIMER2_OLD
SUB W0,W1,W0
SUB VITRI_DATHCL
CLR W0
SUBB VITRI_DATHCH

MOV VITRI_DATHCL,W0
MOV VITRI_DATHCH,W1
MOV VITRI_CNTL,W2
MOV VITRI_CNTH,W3
SUB W0,W2,W4
SUBB W1,W3,W5
BRA C,DUONG

SUB W2,W0,W4
SUBB W3,W1,W5

MOV W4,HS_B
MOV W5,HS_A

MOV BIENTU,W0
MOV W0,HS_C
MOV BIENMAU,W0
MOV W0,HS_D
RCALL HAMTINHTISO
MOV VITRI_CNTL,W2
MOV VITRI_CNTH,W3
MOV KQ_L,W0
MOV KQ_H,W1
SUB W2,W0,W4
SUBB W3,W1,W5

MOV W4,VITRI_DATL
MOV W5,VITRI_DATH
RETURN

DUONG:
MOV W4,HS_B
MOV W5,HS_A
MOV BIENTU,W0
MOV W0,HS_C
MOV BIENMAU,W0
MOV W0,HS_D
RCALL HAMTINHTISO
MOV VITRI_CNTL,W2
MOV VITRI_CNTH,W3
MOV KQ_L,W0
MOV KQ_H,W1
ADD W2,W0,W4
ADDC W3,W1,W5

MOV W4,VITRI_DATL
MOV W5,VITRI_DATH
RETURN
;================================================= =====

HAMTINHTISO:
; KQ:=(AB *C)/D

MOV HS_A,W0
MUL HS_C
MOV W2,W4
MOV W3,W5

MOV HS_B,W0
MUL HS_C

ADD W3,W4,W3
CLR W1
ADDC W5,W1,W4 ; W4,W3,W2 HIT
MOV W2,NHAP2 ; >>T

MOV W3,W6
MOV W4,W7
MOV HS_D,W5 ; 1024
repeat #17
DIV.ud W6,W5 ;Unsigned divide: (Wm + 1:Wm)/Wn ? W0; Rem ? W1
MOV W0,KQ_H

MOV W1,W0 ; SO DU
MOV #0XFFFF,W5
MOV W5,NHAP1
MUL NHAP1 ; >> W3,W2
ADD W2,W1,W2 ;+1 W1
CLR W1
ADDC W3,W1,W3 ;

MOV NHAP2,W1
ADD W2,W1,W6
CLR W1
ADDC W3,W1,W7

MOV HS_D,W5 ; 1024
repeat #17
DIV.ud W6,W5 ;Unsigned divide: (Wm + 1:Wm)/Wn ? W0; Rem ? W1
MOV W0,KQ_L

RETURN

;************************************************* ********
TINH_PIDRW:
MOV SAISO,W0
MUL HESO_P ;>>W3,W2
MOV W2,W6
MOV W3,W7
MOV HESO_PP,W5 ; 1024
repeat #17
DIV.ud W6,W5 ;Unsigned divide: (Wm + 1:Wm)/Wn ? W0; Rem ? W1
MOV W0,KETQUA_P

BTSC BIEN_AMDUONG,#2
RCALL TRUHAM_DRW
BTSS BIEN_AMDUONG,#2
RCALL CONGHAM_DRW

MOV KETQUA_D,W0
MOV W0,HS_B
MOV KETQUA_DH,W0
MOV W0,HS_A
MOV HESO_D,W0
MOV W0,HS_C
MOV HESO_DP,W0
MOV W0,HS_D
RCALL HAMTINHTISO
MOV KQ_L,W0
MOV W0,LUUKETQUA_D

BTSS BIEN_AMDUONG,#0
RCALL TINHHIEU_KI
BTSC BIEN_AMDUONG,#0
RCALL TINHTONG_KI
MOV HESO_I,W0
MUL KETQUA_I
MOV W2,KETQUA_I
BCLR BIEN_AMDUONG,#0

MOV KETQUA_P,W0
MOV W0,OUTPWM
MOV KETQUA_I,W0
BTSS BIEN_AMDUONG,#1
BRA TRRW

ADD OUTPWM

RWOK:
BCLR BIEN_AMDUONG,#3
MOV LUUKETQUA_D,W0
BTSS BIEN_AMDUONG,#2
ADD OUTPWM
BTSC BIEN_AMDUONG,#2
RCALL TRUHAMPP
BTSC BIEN_AMDUONG,#3
BRA KIEMMMR

KMAXR:
MOV OUTPWMMAX,W1
MOV OUTPWM,W0
SUB W1,W0,W2
BRA C,OKL
MOV W1,W0
OKL:
MOV W0,DPWM1
CLR W0
MOV W0,DPWM2
RETURN

TRRW:
SUB OUTPWM
BRA C,RWOK ;>> CONG THEM PD

MOV KETQUA_P,W1
MOV KETQUA_I,W0
SUB W0,W1,W0
MOV W0,OUTPWM

BCLR BIEN_AMDUONG,#3
MOV LUUKETQUA_D,W0
BTSC BIEN_AMDUONG,#2
ADD OUTPWM
BTSS BIEN_AMDUONG,#2 ; THIEU PHAIVIET THEM IF PI#0
RCALL TRUHAMPP ; VAN DUNG
BTSC BIEN_AMDUONG,#3
BRA KMAXR

KIEMMMR:
MOV OUTPWMMAX,W1 ;KIEMMAX
MOV OUTPWM,W0
SUB W1,W0,W2
BRA C,OKLE
MOV W1,W0

OKLE:
MOV W0,DPWM2
CLR W0
MOV W0,DPWM1

RETURN

;**********************************************
TINH_PIDFW:
MOV SAISO,W0
MUL HESO_P ;>>W3,W2
MOV W2,W6
MOV W3,W7
MOV HESO_PP,W5 ; 1024
repeat #17
DIV.ud W6,W5 ;Unsigned divide: (Wm + 1:Wm)/Wn ? W0; Rem ? W1
MOV W0,KETQUA_P

BTSS BIEN_AMDUONG,#2
RCALL TRUHAM_DFW
BTSC BIEN_AMDUONG,#2
RCALL CONGHAM_DFW

MOV KETQUA_D,W0
MOV W0,HS_B
MOV KETQUA_DH,W0
MOV W0,HS_A
MOV HESO_D,W0
MOV W0,HS_C
MOV HESO_DP,W0
MOV W0,HS_D
RCALL HAMTINHTISO
MOV KQ_L,W0
MOV W0,LUUKETQUA_D

BTSC BIEN_AMDUONG,#0
RCALL TINHHIEU_KI
BTSS BIEN_AMDUONG,#0
RCALL TINHTONG_KI
MOV HESO_I,W0
MUL KETQUA_I
MOV W2,KETQUA_I
BSET BIEN_AMDUONG,#0

MOV KETQUA_P,W0
MOV W0,OUTPWM
MOV KETQUA_I,W0
BTSS BIEN_AMDUONG,#1
BRA TRFW

ADD OUTPWM
FWOK:
BCLR BIEN_AMDUONG,#3
MOV LUUKETQUA_D,W0
BTSC BIEN_AMDUONG,#2
ADD OUTPWM
BTSS BIEN_AMDUONG,#2
RCALL TRUHAMPP
BTSC BIEN_AMDUONG,#3
BRA KIEMMMF

KMAXF:
MOV OUTPWMMAX,W1
MOV OUTPWM,W0
SUB W1,W0,W2
BRA C,OKLR
MOV W1,W0
OKLR:
MOV W0,DPWM2
CLR W0
MOV W0,DPWM1
RETURN
TRFW:
SUB OUTPWM
BRA C,FWOK

MOV KETQUA_P,W2
MOV KETQUA_I,W1
SUB W1,W2,W0
MOV W0,OUTPWM

BCLR BIEN_AMDUONG,#3
MOV LUUKETQUA_D,W0
BTSS BIEN_AMDUONG,#2
ADD OUTPWM
BTSC BIEN_AMDUONG,#2 ; THIEU PHAIVIET THEM IF PI#0
RCALL TRUHAMPP ;VAN DUNG
BTSC BIEN_AMDUONG,#3
BRA KMAXF

KIEMMMF:
MOV OUTPWMMAX,W1
MOV OUTPWM,W0
SUB W1,W0,W2
BRA C,OKLV
MOV W1,W0
OKLV:
MOV W0,DPWM1
CLR W0
MOV W0,DPWM2

RETURN

;00000000000000000000000000000000000000000000
TRUHAMPP:
MOV OUTPWM,W1
SUB W1,W0,W2
BRA C,FGHJ
SUB W0,W1,W2
MOV W2,OUTPWM
BSET BIEN_AMDUONG,#3
RETURN
FGHJ:
MOV W2,OUTPWM

RETURN

;***********************************************
TINHHIEU_KI:
MOV SAISO,W0
MOV SAISO_CU,W1
SUB W0,W1,W2
BRA C,DRW

SUB W1,W0,W2
MOV W2,KETQUA_I
BCLR BIEN_AMDUONG,#1 ; DANGAM
MOV SAISO,W0
MOV W0,SAISO_CU
RETURN
DRW:
BSET BIEN_AMDUONG,#1
MOV W2,KETQUA_I ; DUONG
MOV SAISO,W0
MOV W0,SAISO_CU
RETURN
;======================
TINHTONG_KI:
MOV SAISO,W0
MOV W0,KETQUA_I
MOV SAISO_CU,W0
ADD KETQUA_I
BSET BIEN_AMDUONG,#1 ;DANG DUONG
MOV SAISO,W0
MOV W0,SAISO_CU
RETURN
;========================
TRUHAM_DRW:
MOV SAISO,W0
MOV KETQUA_D,W1
MOV #0,W3
MOV KETQUA_DH,W4
SUB W0,W1,W2
SUBB W3,W4,W5
BRA C,OKTRUD

SUB W1,W0,W2
CLR W0
SUBB KETQUA_DH ;>>DH
MOV W2,KETQUA_D
BSET BIEN_AMDUONG,#2
RETURN
OKTRUD:
MOV W2,KETQUA_D
MOV W5,KETQUA_DH
BCLR BIEN_AMDUONG,#2
RETURN

;========================
CONGHAM_DRW:
MOV SAISO,W0
ADD KETQUA_D
CLR W0
ADDC KETQUA_DH
BCLR BIEN_AMDUONG,#2
RETURN

;===============================================
TRUHAM_DFW:
MOV SAISO,W0
MOV KETQUA_D,W1
MOV #0,W3
MOV KETQUA_DH,W4
SUB W0,W1,W2
SUBB W3,W4,W5
BRA C,OKTRUDF

SUB W1,W0,W2
CLR C
SUBB KETQUA_DH
MOV W2,KETQUA_D
BCLR BIEN_AMDUONG,#2
RETURN
OKTRUDF:
MOV W2,KETQUA_D
MOV W5,KETQUA_DH
BSET BIEN_AMDUONG,#2
RETURN

;========================
CONGHAM_DFW:
MOV SAISO,W0
ADD KETQUA_D
CLR W0
ADDC KETQUA_DH
BSET BIEN_AMDUONG,#2
RETURN

;8888888888888888888888888888888888888888888888888 8888888888888888888888
XACDINHHESO_K_I_D:
MOV GIAN,W0
MOV #1000,W1
SUB W0,W1,W2
BRA C,RAKP

SUB W1,W0,W0
MOV #10,W1
MOV W1,HESO_P
MOV W1,HESO_PP
ADD HESO_PP ;>0.01<KP<100
RETURN
RAKP:
MOV #10,W1
MOV W1,HESO_P
MOV W1,HESO_PP
MOV W2,W0
ADD HESO_P
RETURN

;8888888888888888888888888888888888888888888888888 8888888888888888888888

XACDINHHESO_HESOD:
MOV GIAN_D,W0
MOV #1500,W1
SUB W0,W1,W2
BRA C,RAKPD

SUB W1,W0,W0
MOV #1,W1
MOV W1,HESO_D
MOV W1,HESO_DP
ADD HESO_DP ;>0.01<KP<100
RETURN
RAKPD:
MOV #1,W1
MOV W1,HESO_D
MOV W1,HESO_DP
MOV W2,W0
ADD HESO_D
RETURN
;99999999999999999999999999999999999999999999
HIEUCHINH_GEAR:

MOV #0,W0
MOV W0,VITRI_DATHCL
MOV W0,VITRI_CNTL
MOV W0,VITRI_DATL
MOV W0,VITRI_EL

MOV #0X7FFF,W0
MOV W0,VITRI_DATHCH
MOV W0,VITRI_CNTH
MOV W0,VITRI_DATH
MOV W0,VITRI_EH
RETURN

;8888888888888888888888888888888888888888888888888 8888888888888888888888

XACDINHHESO_HESOI:
MOV GIAN_I,W0
MOV W0,HESO_I
RETURN

;************************************************* ******************
DOCADC:
btss IFS0, #ADIF
RETURN
bclr IFS0, #ADIF
MOV ADCBUF0, W3

; MOV W3,HESO_I

MOV #10,W4

repeat #17
DIV.S W3,W4
MOV W0,HESO_P

RETURN
;************************************************* ***************88********************************* **********************************
;***************************CHUONG TRINH SU DUNG HIEN THI LED********************************************888 888888888888**********************

BANPHIM:
BTSC DOCD,#NUT1
BRA PH2
RCALL SULIPRO
RETURN
PH2:
BTSC DOCB,#NUT4
BRA PH3
RCALL SULIGIAM
RETURN
PH3:
BTSC DOCB,#NUT2
BRA PH4
RCALL SULITANG
RETURN
PH4:
BTSC DOCB,#NUT3
BRA NOTPHIM
RCALL SULIDATA_MOD
RETURN

NOTPHIM:
CLR W0
MOV W0,BIENPHIM

MOV #4,W0
MOV W0,THOIGIANPHIM1
MOV #20,W0
MOV W0,THOIGIANPHIM2
MOV #7,W0
MOV W0,THOIGIANPHIM3
MOV #32,W0
MOV W0,LUUTHOIGIAN

MOV #2000,W0
MOV W0,THOIGIANPHIMPR

RETURN

;------------------------------------
SULIPRO:

dec THOIGIANPHIMPR
bra NZ,CHUAPR
MOV #50000,W0 ;2000
MOV W0,THOIGIANPHIMPR

BTSC BIENPHIM,#NUT1
RETURN
BSET BIENPHIM,#NUT1

MOV BIENHIENTHI,W0
MOV #1,W1
CPSGT W0,W1
BRA DANGCHAY
MOV BIENMOD,W0
MOV W0,BIENHIENTHI
BCLR BIENHAM_DATA,#0
rcall GHI_EEPROM
RCALL DATMAUTOCDO

RETURN
DANGCHAY:
MOV #2,W0
MOV W0,BIENHIENTHI
BSET BIENHAM_DATA,#0

CHUAPR:
RETURN
;*********************************
SULIGIAM:
MOV BIENHIENTHI,W0
MOV #1,W1
CPSGT W0,W1 ;; SKIP IF >
RETURN

BTSS BIENHAM_DATA,#0
BRA GDATA
MOV BIENHIENTHI,W0
MOV #2,W1
CPSGT W0,W1
RETURN
MOV W0,NHAP1
RCALL HAMGIAM
MOV NHAP1,W0
MOV W0,BIENHIENTHI
RETURN
GDATA:
MOV BIENHIENTHI,W0
MOV #2,W1
CPSNE W0,W1
BRA GTHT2

MOV #3,W1
CPSNE W0,W1
BRA GTHT3

MOV #4,W1
CPSNE W0,W1
BRA GTHT4

MOV #5,W1
CPSNE W0,W1
BRA GTHT5

MOV #6,W1
CPSNE W0,W1
BRA GTHT6

MOV #7,W1
CPSNE W0,W1
BRA GTHT7

MOV #8,W1
CPSNE W0,W1
BRA GTHT8

GTHT2:
MOV BIENMAU,W0
MOV #1,W1
CPSGT W0,W1 ; SKIP IF >
RETURN
MOV W0,NHAP1
RCALL HAMGIAM
MOV NHAP1,W0
MOV W0,BIENMAU
RCALL HIEUCHINH_GEAR

RETURN
GTHT3:
MOV BIENTU,W0
MOV #1,W1
CPSGT W0,W1 ;; SKIP IF >
RETURN
MOV W0,NHAP1
RCALL HAMGIAM
MOV NHAP1,W0
MOV W0,BIENTU
RCALL HIEUCHINH_GEAR

RETURN

GTHT4:
MOV GIAN,W0
MOV #0,W1
CPSGT W0,W1 ;; SKIP IF >
RETURN
MOV W0,NHAP1
RCALL HAMGIAM
MOV NHAP1,W0
MOV W0, GIAN
RCALL XACDINHHESO_K_I_D
RETURN

GTHT5:
MOV NHAN_ENCODER,W0
MOV #0,W1
CPSGT W0,W1 ;; SKIP IF >
RETURN
MOV W0,NHAP1
RCALL HAMGIAM
MOV NHAP1,W0
MOV W0, NHAN_ENCODER
RCALL THONGSO_ENCODER
RETURN

GTHT6:
MOV GIAN_D,W0
MOV #1,W1
CPSGT W0,W1 ;; SKIP IF >
RETURN
MOV W0,NHAP1
RCALL HAMGIAM
MOV NHAP1,W0
MOV W0,GIAN_D
RCALL XACDINHHESO_HESOD

RETURN

GTHT7:
MOV GIAN_I,W0
MOV #0,W1
CPSGT W0,W1 ;; SKIP IF >
RETURN
MOV W0,NHAP1
RCALL HAMGIAM
MOV NHAP1,W0
MOV W0,GIAN_I
RCALL XACDINHHESO_HESOI

RETURN

GTHT8:
MOV LUUGOCZEZO,W0
MOV #1,W1
CPSGT W0,W1 ;; SKIP IF >
RETURN
MOV W0,NHAP1
RCALL HAMGIAM
MOV NHAP1,W0
MOV W0,LUUGOCZEZO
RETURN

;**********************************
SULITANG:

MOV BIENHIENTHI,W0
MOV #1,W1
CPSGT W0,W1 ;; SKIP IF >
RETURN
BTSS BIENHAM_DATA,#0
BRA TDATA

MOV BIENHIENTHI,W0
MOV #8,W1
CPSLT W0,W1 ; SKIP IF <
RETURN
MOV W0,NHAP1
RCALL HAMTANG
MOV NHAP1,W0
MOV W0,BIENHIENTHI
RETURN
TDATA:
MOV BIENHIENTHI,W0
MOV #2,W1
CPSNE W0,W1
BRA THT2

MOV #3,W1
CPSNE W0,W1
BRA THT3

MOV #4,W1
CPSNE W0,W1
BRA THT4

MOV #5,W1
CPSNE W0,W1
BRA THT5

MOV #6,W1
CPSNE W0,W1
BRA THT6

MOV #7,W1
CPSNE W0,W1
BRA THT7

MOV #8,W1
CPSNE W0,W1
BRA THT8

THT2:
MOV BIENMAU,W0
MOV #9999,W1
CPSLT W0,W1 ;; SKIP IF <
RETURN
MOV W0,NHAP1
RCALL HAMTANG
MOV NHAP1,W0
MOV W0,BIENMAU
RCALL HIEUCHINH_GEAR
RETURN
THT3:
MOV BIENTU,W0
MOV #9999,W1
CPSLT W0,W1 ; SKIP IF <
RETURN
MOV W0,NHAP1
RCALL HAMTANG
MOV NHAP1,W0
MOV W0,BIENTU
RCALL HIEUCHINH_GEAR
RETURN

THT4:
MOV GIAN,W0
MOV #2000,W1
CPSLT W0,W1 ; SKIP IF <
RETURN
MOV W0,NHAP1
RCALL HAMTANG
MOV NHAP1,W0
MOV W0, GIAN
RCALL XACDINHHESO_K_I_D
RETURN

THT5:
MOV NHAN_ENCODER,W0
MOV #1,W1
CPSLT W0,W1 ; SKIP IF <
RETURN
MOV W0,NHAP1
RCALL HAMTANG
MOV NHAP1,W0
MOV W0, NHAN_ENCODER
RCALL THONGSO_ENCODER

RETURN

THT6:
MOV GIAN_D,W0
MOV #2000,W1
CPSLT W0,W1 ; SKIP IF <
RETURN
MOV W0,NHAP1
RCALL HAMTANG
MOV NHAP1,W0
MOV W0,GIAN_D
RCALL XACDINHHESO_HESOD

RETURN

THT7:
MOV GIAN_I,W0
MOV #1000,W1
CPSLT W0,W1 ; SKIP IF <
RETURN
MOV W0,NHAP1
RCALL HAMTANG
MOV NHAP1,W0
MOV W0,GIAN_I
RCALL XACDINHHESO_HESOI

RETURN

THT8:
MOV LUUGOCZEZO,W0
MOV #320,W1
CPSLT W0,W1 ; SKIP IF <
RETURN
MOV W0,NHAP1
RCALL HAMTANG
MOV NHAP1,W0
MOV W0,LUUGOCZEZO
RETURN

;**********************************
SULIDATA_MOD:

BTSC BIENPHIM,#NUT4
RETURN
BSET BIENPHIM,#NUT4
MOV BIENHIENTHI,W0
MOV #1,W1
CPSGT W0,W1 ; SKIP IF >
BRA MODDANGCHAY

COM BIENHAM_DATA
MOV #1,W0
AND BIENHAM_DATA
RETURN

MODDANGCHAY:
COM BIENMOD
MOV #1,W0
AND BIENMOD
MOV BIENMOD,W0
MOV W0,BIENHIENTHI

RCALL GHI_EEPROM_BIENMOD

RETURN

;***********************************************
HAMGIAM:
dec THOIGIANPHIM1
bra NZ,CHUAGIAM
MOV #100,W0
MOV W0,THOIGIANPHIM1

dec THOIGIANPHIM2
bra NZ,CHUAGIAM
MOV LUUTHOIGIAN,W0
MOV W0,THOIGIANPHIM2

DEC NHAP1

dec THOIGIANPHIM3
bra NZ,CHUAGIAM
MOV #5,W0
MOV W0,THOIGIANPHIM3

dec2 LUUTHOIGIAN
bra NZ,CHUAGIAM
MOV #2,W0
MOV W0,LUUTHOIGIAN

RETURN
CHUAGIAM:
RETURN
;**************
;-----------------
HAMTANG:
dec THOIGIANPHIM1
bra NZ,CHUATANG
MOV #100,W0
MOV W0,THOIGIANPHIM1

dec THOIGIANPHIM2
bra NZ,CHUATANG
MOV LUUTHOIGIAN,W0
MOV W0,THOIGIANPHIM2

INC NHAP1

dec THOIGIANPHIM3
bra NZ,CHUATANG
MOV #5,W0
MOV W0,THOIGIANPHIM3

dec2 LUUTHOIGIAN
bra NZ,CHUATANG
MOV #2,W0
MOV W0,LUUTHOIGIAN

RETURN
CHUATANG:

RETURN
;----------------------

;************************************************* *
GHI_EEPROM:
MOV #0x4045,W0 ; KHOI =4045/44
MOV W0,NVMCON

MOV #0XFe00,W0 ; Init pointer
MOV #0X007F,W1
MOV W1,NVMADRU
MOV W0,NVMADR

;Start erase cycle by setting WR after writing key sequence
DISI #5 ; Block all interrupts
MOV #0x55,W0 ;
MOV W0,NVMKEY ; Write the 0x55 key
MOV #0xAA,W1 ;
MOV W1,NVMKEY ; Write the 0xAA key
BSET NVMCON,#WR ; Initiate erase sequence
NOP
NOP
DAYL2:
BTSC NVMCON,#WR
BRA DAYL2

MOV #0XFe00,W0 ; Init pointer
MOV #0X7F,W1
MOV W1,TBLPAG
MOV BIENMAU,W2 ; Get 1st data
TBLWTL W2,[ W0]

MOV #0x4004,W0 ; KHOI =400A/4004
MOV W0,NVMCON
DISI #5 ; Block all interrupts with priority <7
MOV #0x55,W0
MOV W0,NVMKEY ; Write the 0x55 key
MOV #0xAA,W1
MOV W1,NVMKEY ; Write the 0xAA key
BSET NVMCON,#WR ; Initiate program sequence
NOP
NOP
DAYL3:
BTSC NVMCON,#WR
BRA DAYL3

MOV #0XFe02,W0 ; Init pointer
MOV #0X7F,W1
MOV W1,TBLPAG
MOV BIENTU,W2 ; Get 1st data
TBLWTL W2,[ W0]

MOV #0x4004,W0 ; KHOI =400A/4004
MOV W0,NVMCON
DISI #5 ; Block all interrupts with priority <7
MOV #0x55,W0
MOV W0,NVMKEY ; Write the 0x55 key
MOV #0xAA,W1
MOV W1,NVMKEY ; Write the 0xAA key
BSET NVMCON,#WR ; Initiate program sequence
NOP
NOP
DAYL4:
BTSC NVMCON,#WR
BRA DAYL4

MOV #0XFe04,W0 ; Init pointer
MOV #0X7F,W1
MOV W1,TBLPAG
MOV GIAN,W2 ; Get 1st data
TBLWTL W2,[ W0]

MOV #0x4004,W0 ; KHOI =400A/4004
MOV W0,NVMCON
DISI #5 ; Block all interrupts with priority <7
MOV #0x55,W0
MOV W0,NVMKEY ; Write the 0x55 key
MOV #0xAA,W1
MOV W1,NVMKEY ; Write the 0xAA key
BSET NVMCON,#WR ; Initiate program sequence
NOP
NOP
DAYL5:
BTSC NVMCON,#WR
BRA DAYL5

MOV #0XFe06,W0 ; Init pointer
MOV #0X7F,W1
MOV W1,TBLPAG
MOV NHAN_ENCODER,W2 ; Get 1st data
TBLWTL W2,[ W0]

MOV #0x4004,W0 ; KHOI =400A/4004
MOV W0,NVMCON
DISI #5 ; Block all interrupts with priority <7
MOV #0x55,W0
MOV W0,NVMKEY ; Write the 0x55 key
MOV #0xAA,W1
MOV W1,NVMKEY ; Write the 0xAA key
BSET NVMCON,#WR ; Initiate program sequence
NOP
NOP
DAYL6:
BTSC NVMCON,#WR
BRA DAYL6

MOV #0XFe08,W0 ; Init pointer
MOV #0X7F,W1
MOV W1,TBLPAG
MOV BIENMOD,W2 ; Get 1st data
TBLWTL W2,[ W0]

MOV #0x4004,W0 ; KHOI =400A/4004
MOV W0,NVMCON
DISI #5 ; Block all interrupts with priority <7
MOV #0x55,W0
MOV W0,NVMKEY ; Write the 0x55 key
MOV #0xAA,W1
MOV W1,NVMKEY ; Write the 0xAA key
BSET NVMCON,#WR ; Initiate program sequence
NOP
NOP
DAYL3DD:
BTSC NVMCON,#WR
BRA DAYL3DD

MOV #0XFe0A,W0 ; Init pointer
MOV #0X7F,W1
MOV W1,TBLPAG
MOV GIAN_D,W2 ; Get 1st data
TBLWTL W2,[ W0]

MOV #0x4004,W0 ; KHOI =400A/4004
MOV W0,NVMCON
DISI #5 ; Block all interrupts with priority <7
MOV #0x55,W0
MOV W0,NVMKEY ; Write the 0x55 key
MOV #0xAA,W1
MOV W1,NVMKEY ; Write the 0xAA key
BSET NVMCON,#WR ; Initiate program sequence
NOP
NOP
DAYL7:
BTSC NVMCON,#WR
BRA DAYL7

MOV #0XFe0C,W0 ; Init pointer
MOV #0X7F,W1
MOV W1,TBLPAG
MOV GIAN_I,W2 ; Get 1st data
TBLWTL W2,[ W0]

MOV #0x4004,W0 ; KHOI =400A/4004
MOV W0,NVMCON
DISI #5 ; Block all interrupts with priority <7
MOV #0x55,W0
MOV W0,NVMKEY ; Write the 0x55 key
MOV #0xAA,W1
MOV W1,NVMKEY ; Write the 0xAA key
BSET NVMCON,#WR ; Initiate program sequence
NOP
NOP
DAYL8:
BTSC NVMCON,#WR
BRA DAYL8

MOV #0XFe0E,W0 ; Init pointer
MOV #0X7F,W1
MOV W1,TBLPAG
MOV LUUGOCZEZO,W2 ; Get 1st data
TBLWTL W2,[ W0]

MOV #0x4004,W0 ; KHOI =400A/4004
MOV W0,NVMCON
DISI #5 ; Block all interrupts with priority <7
MOV #0x55,W0
MOV W0,NVMKEY ; Write the 0x55 key
MOV #0xAA,W1
MOV W1,NVMKEY ; Write the 0xAA key
BSET NVMCON,#WR ; Initiate program sequence
NOP
NOP
DAYL9:
BTSC NVMCON,#WR
BRA DAYL9

RETURN
;************************************************

GHI_EEPROM_BIENMOD:

MOV #0x4044,W0 ; KHOI =4045/44
MOV W0,NVMCON

MOV #0XFe08,W0 ; Init pointer
MOV #0X007F,W1
MOV W1,NVMADRU
MOV W0,NVMADR

;Start erase cycle by setting WR after writing key sequence
DISI #5 ; Block all interrupts
MOV #0x55,W0 ;
MOV W0,NVMKEY ; Write the 0x55 key
MOV #0xAA,W1 ;
MOV W1,NVMKEY ; Write the 0xAA key
BSET NVMCON,#WR ; Initiate erase sequence
NOP
NOP
DAYLD:
BTSC NVMCON,#WR
BRA DAYLD

MOV #0XFe08,W0 ; Init pointer
MOV #0X7F,W1
MOV W1,TBLPAG
MOV BIENMOD,W2 ; Get 1st data
TBLWTL W2,[ W0]

MOV #0x4004,W0 ; KHOI =400A/4004
MOV W0,NVMCON
DISI #5 ; Block all interrupts with priority <7
MOV #0x55,W0
MOV W0,NVMKEY ; Write the 0x55 key
MOV #0xAA,W1
MOV W1,NVMKEY ; Write the 0xAA key
BSET NVMCON,#WR ; Initiate program sequence
NOP
NOP
DAYL3D:
BTSC NVMCON,#WR
BRA DAYL3D
RETURN
;=================================================

DOC_EEPROM:
MOV #0Xfe00,W0 ; Init pointer
MOV #0X007F,W1
MOV W1,TBLPAG
TBLRDL [W0], W2 ; read data EEPROM
MOV W2,BIENMAU

MOV #0Xfe02,W0 ; Init pointer
MOV #0X007F,W1
MOV W1,TBLPAG
TBLRDL [W0], W2 ; read data EEPROM
MOV W2,BIENTU

MOV #0Xfe04,W0 ; Init pointer
MOV #0X007F,W1
MOV W1,TBLPAG
TBLRDL [W0], W2 ; read data EEPROM
MOV W2, GIAN

MOV #0Xfe06,W0 ; Init pointer
MOV #0X007F,W1
MOV W1,TBLPAG
TBLRDL [W0], W2 ; read data EEPROM
MOV W2, NHAN_ENCODER

MOV #0Xfe08,W0 ; Init pointer
MOV #0X007F,W1
MOV W1,TBLPAG
TBLRDL [W0], W2 ; read data EEPROM
MOV W2,BIENMOD

MOV #0Xfe0A,W0 ; Init pointer
MOV #0X007F,W1
MOV W1,TBLPAG
TBLRDL [W0], W2 ; read data EEPROM
MOV W2,GIAN_D

MOV #0Xfe0C,W0 ; Init pointer
MOV #0X007F,W1
MOV W1,TBLPAG
TBLRDL [W0], W2 ; read data EEPROM
MOV W2,GIAN_I

MOV #0Xfe0E,W0 ; Init pointer
MOV #0X007F,W1
MOV W1,TBLPAG
TBLRDL [W0], W2 ; read data EEPROM
MOV W2,LUUGOCZEZO

MOV GIAN_D,W0
MOV #0,W1
CPSNE W0,W1 ; PHAI #0
INC GIAN_D

MOV #100,W2
MOV BIENMAU,W0
MOV #0,W1
CPSNE W0,W1 ; PHAI #0
MOV W2,BIENMAU

RETURN

;================================================= ===

DATMAUTOCDO:
MOV #9162,W0
MUL GIAN_I
MOV w3, PR3 ; with the value 0xFFFF
MOV w2, PR2 ; Load the Period Register2 with value 0xFFFF
CLR TMR1
CLR TMR3HLD
CLR TMR2

MOV #6000,W5
MOV GIAN_D,W4
repeat #17
DIV.U W5,W4 ;W5/W4=W0; REM=W1
MOV W0,HESOTHOIGIAN
RETURN

;************************************************* *
DELAY:
MOV #500,W0
MOV W0,SODEM
LOPDL:
DEC SODEM
bra NZ, LOPDL
RETURN

KHOITAOLCD:
RCALL DELAY
BCLR LATB,#8
BCLR LATB,#7
MOV #0B0011,W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
RCALL DELAY

BSET LATB,#8
BCLR LATB,#8
RCALL DELAY

BSET LATB,#8
BCLR LATB,#8
RCALL DELAY

MOV #0B0010,W0
MOV W0,LATD ;SET 4BIT
BSET LATB,#8
BCLR LATB,#8
RCALL DELAY

MOV #0B0000,W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
MOV #0B1100,W0 ;display
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
RCALL DELAY

MOV #0B0000,W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
MOV #0B0011,W0 ;shile mod display
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
RCALL DELAY

MOV #0B0000,W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
MOV #0B0001,W0 ;CLR
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
RCALL DELAY

MOV #0B0000,W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
MOV #0B0010,W0 ; GO HOME
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
RCALL DELAY

BSET LATB,#7
CLR W3
MOV W3,NHAP1

NAPCHU:
MOV NHAP1,W3
mov #tblpage(SineTable1),W0
mov W0,TBLPAG
mov #tbloffset(SineTable1),W0
ADD W0,W3,W1
TBLRDL [W1],W0 ; HE SO SINS NAM TRONG W0
SWAP.B W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
SWAP.B W0
MOV W0,LATD

BSET LATB,#8
BCLR LATB,#8
REPEAT #2000
NOP
INC2 NHAP1
MOV NHAP1,W0
MOV #160,W2
CPSEQ W0,W2
BRA NAPCHU

RCALL DELAY
BCLR LATB,#7
MOV #0B1000,W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
MOV #0B1010,W0 ;ad ram
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
RCALL DELAY

RETURN

BSET LATB,#7
MOV #'7',W0
SWAP.B W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
SWAP.B W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
REPEAT #1000
NOP

BSET LATB,#7
MOV #'6',W0
SWAP.B W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
SWAP.B W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
REPEAT #1000
NOP
BSET LATB,#7
MOV #'5',W0
SWAP.B W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
SWAP.B W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
REPEAT #1000
NOP

RETURN

;888888888888888888888888888888888888888888888888
HIENTHILCD:
MOV TROLCD1,W0
MOV #1,W1
CPSEQ W0,W1 ; SKIP IF =
BRA SETRAM

INC2 TROLCD1
BSET LATB,#7

MOV TROLCD,W0

MOV #0,W1
CPSNE W0,W1
MOV BCDT1,W2
MOV #2,W1

CPSNE W0,W1
MOV BCDT2,W2
MOV #4,W1

CPSNE W0,W1
MOV BCDT3,W2
MOV #6,W1

CPSNE W0,W1
MOV BCDT4,W2
MOV #8,W1

CPSNE W0,W1
MOV BCDT5,W2
MOV #10,W1

CPSNE W0,W1
MOV BCDV1,W2
MOV #12,W1

CPSNE W0,W1
MOV BCDV2,W2
MOV #14,W1

CPSNE W0,W1
MOV BCDV3,W2
MOV #16,W1

CPSNE W0,W1
MOV BCDV4,W2
MOV #18,W1

CPSNE W0,W1
MOV BCDV5,W2
MOV #20,W1

CPSNE W0,W1
MOV BCDD1,W2
MOV #22,W1

CPSNE W0,W1
MOV BCDD2,W2
MOV #24,W1

CPSNE W0,W1
MOV BCDD3,W2
MOV #26,W1

CPSNE W0,W1
MOV BCDD4,W2
MOV #28,W1

CPSNE W0,W1
MOV BCDD5,W2
MOV #30,W1

CPSNE W0,W1
MOV BCDA1,W2
MOV #32,W1

CPSNE W0,W1
MOV BCDA2,W2
MOV #34,W1

CPSNE W0,W1
MOV BCDA3,W2
MOV #36,W1

CPSNE W0,W1
MOV BCDA4,W2
MOV #38,W1

CPSNE W0,W1
MOV BCDA5,W2

MOV #2,W0
MOV W0,NHAP1
MOV W2,W0
MUL NHAP1 ;=W3W2

MOV W2,W3
mov #tblpage(SineTable3),W0
mov W0,TBLPAG
mov #tbloffset(SineTable3),W0
ADD W0,W3,W1
TBLRDL [W1],W0 ; HE SO SINS NAM TRONG W0

SWAP.B W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
SWAP.B W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
RETURN

SETRAM:
MOV #1,W0
MOV W0,TROLCD1
BCLR LATB,#7

INC2 TROLCD
MOV TROLCD,W0
MOV #39,W2
CPSLT W0,W2 ; SKIP IF <
BRA XOATROLCD
KOTROLCD:
MOV TROLCD,W3
mov #tblpage(SineTable4),W0
mov W0,TBLPAG
mov #tbloffset(SineTable4),W0
ADD W0,W3,W1
TBLRDL [W1],W0 ; HE SO SINS NAM TRONG W0

BSET W0,#7
SWAP.B W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
SWAP.B W0 ;ad ram
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
RETURN

XOATROLCD:
CLR W0
MOV W0,TROLCD

MOV #0B1000,W0
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
MOV #0B1010,W0 ;ad ram
MOV W0,LATD
BSET LATB,#8
BCLR LATB,#8
RETURN

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;;;;;;;;;;
HIENTHISO:

; DEC DEMLCD
; BRA NZ,main_loop
; MOV W0,DEMLCD

MOV POSCNT,W5
RCALL HEXTOBCD
MOV BCD1,W0
MOV W0,BCDT1
MOV BCD2,W0
MOV W0,BCDT2
MOV BCD3,W0
MOV W0,BCDT3
MOV BCD4,W0
MOV W0,BCDT4
MOV BCD5,W0
MOV W0,BCDT5

MOV VITRI_DATL,W5
RCALL HEXTOBCD
MOV BCD1,W0
MOV W0,BCDV1
MOV BCD2,W0
MOV W0,BCDV2
MOV BCD3,W0
MOV W0,BCDV3
MOV BCD4,W0
MOV W0,BCDV4
MOV BCD5,W0
MOV W0,BCDV5

; MOV HIEUFW,W5
; MOV POSCNT1,W5
; MOV XUNGCHAY1,W5
MOV DPWM1,W5

RCALL HEXTOBCD
MOV BCD1,W0
MOV W0,BCDD1
MOV BCD2,W0
MOV W0,BCDD2
MOV BCD3,W0
MOV W0,BCDD3
MOV BCD4,W0
MOV W0,BCDD4
MOV BCD5,W0
MOV W0,BCDD5

; MOV HIEURW,W5
; MOV POSCNT0,W5
MOV DPWM2,W5

RCALL HEXTOBCD
MOV BCD1,W0
MOV W0,BCDA1
MOV BCD2,W0
MOV W0,BCDA2
MOV BCD3,W0
MOV W0,BCDA3
MOV BCD4,W0
MOV W0,BCDA4
MOV BCD5,W0
MOV W0,BCDA5
RCALL HIENTHILCD

RETURN

;99999999999999999999999999999999999999999999999
HEXTOBCD:
MOV W0,W5
; IN W5
MOV #10000,W4
repeat #17
DIV.U W5,W4 ;W5/W4=W0; REM=W1
MOV W0,BCD1

MOV W1,W5
MOV #1000,W4
repeat #17
DIV.U W5,W4 ;W5/W4=W0; REM=W1
MOV W0,BCD2

MOV W1,W5
MOV #100,W4
repeat #17
DIV.U W5,W4 ;W5/W4=W0; REM=W1
MOV W0,BCD3

MOV W1,W5
MOV #10,W4
repeat #17
DIV.U W5,W4 ;W5/W4=W0; REM=W1
MOV W0,BCD4
MOV W1,BCD5
RETURN
;------------------------------------------------

;DELAY:
; MOV #1000,W0 ;100
; MOV W0,NHAP3
;
;LOPDL:
; REPEAT #1000;
; NOP
; DEC NHAP3
; BRA NZ,LOPDL
;RETURN

;************************************************* ***************************

KHOIDONGDIVERSERVO:

MOV #900,W0 ;700
MOV W0,OUTPWMMAX

MOV #0,W0 ;700
MOV W0,OUTPWM

MOV #30,W0 ;50
MOV W0,GIATRIOFSET
MOV #900,W0 ; TRU 20% DE NAP TU NGUON KICH ;1300
MOV GIATRIOFSET,W1
SUB W0,W1,W1
MOV W1,GIUPTR

MOV #TUSO,W0
MOV W0,BIENTU
MOV #MAUSO,W0
MOV W0,BIENMAU

MOV #0,W0
MOV W0,VITRI_DATHCL
MOV W0,VITRI_CNTL
MOV W0,VITRI_DATL
MOV W0,VITRI_EL

MOV #0X7FFF,W0
MOV W0,VITRI_DATHCH
MOV W0,VITRI_CNTH
MOV W0,VITRI_DATH
MOV W0,VITRI_EH

MOV #0,W0
MOV W0,TIMER1_OLD
MOV W0,TIMER2_OLD
MOV W0,BIEN_AMDUONG
MOV W0,SAISO
MOV W0,SAISO_CU

MOV W0,KETQUA_P
MOV W0,KETQUA_I
MOV W0,KETQUA_D
MOV W0,KETQUA_DH

CLR TMR2
CLR TMR1

MOV #35,W0 ; 8
MOV W0,HESO_P

MOV #0,W0 ;50
MOV W0,HESO_I

MOV #1,W0 ;1
MOV W0,HESO_D

MOV #500,W0 ;500
MOV W0,HESO_DP

MOV #65000,W0
MOV W0,THOIGIAN1
MOV #65000,W0
MOV W0,THOIGIAN2
MOV #10000,W0
MOV W0,THOIGIAN3

MOV #10,W0
MOV W0,THOIGIAN4

BSET BIEN_AMDUONG,#10

MOV #1000,W0
MOV W0,THOIGIANBAOVE
MOV #1000,W0
MOV W0,THOIGIANBAOVEDONG

;================================================= =======
MOV #50,W0
MOV W0,THOIGIANLED

MOV #5289,W0
RCALL TINHCHUNG

MOV #1,W0
MOV W0,SODEMLED

MOV #7,W0
MOV W0,THOIGIANPHIM1
MOV #15,W0
MOV W0,THOIGIANPHIM2
MOV #5,W0
MOV W0,THOIGIANPHIM3
MOV #10,W0
MOV W0,LUUTHOIGIAN

MOV #0,W0
MOV W0,BIENHIENTHI
MOV W0,BIENMOD
MOV W0,GOCNGHIENGTHUC
MOV W0,BIENPHIM
MOV W0,BIENHAM_DATA
MOV W0,GOCAMDUONG

CALL DOC_EEPROM
RCALL HIEUCHINH_EEROM
RCALL XACDINHHESO_K_I_D
RCALL XACDINHHESO_HESOD
RCALL XACDINHHESO_HESOI

RETURN

;************************************************* ************************************************** ******************************

;-----------------------------------------------------------------------------
;Chuong trinh con khoi tao TMR1
;TMR1 duoc khoi tao de tran sau moi 1 giay tai muc xung 8 Mips (fcy = 8 MHz)
;------------------------------------------------------------------------------

Init_TMR1:
CLR T1CON ; Stops the Timer1 and reset control reg.
CLR TMR1 ; Clear contents of the timer register
MOV #0XFFFF, w0 ; Load the Period register
MOV w0, PR1 ; with the value 0x8CFF
;BSET IPC0, #T1IP0 ; Setup Timer1 interrupt for
;BCLR IPC0, #T1IP1 ; desired priority level
;BCLR IPC0, #T1IP2 ; (this example assigns level 1 priority)
;BCLR IFS0, #T1IF ; Clear the Timer1 interrupt status flag
;BSET IEC0, #T1IE ; Enable Timer1 interrupts
MOV #0x8016, W0 ; Start Timer1 with prescaler settings at
; 1:8 and clock source set to the external
MOV w0, T1CON ; clock in the synchronous mode
; Example code for Timer1 ISR
BCLR T1CON, #TCKPS0
BCLR T1CON, #TCKPS1
RETURN
return

Init_TMR2:
CLR T2CON ; Stops the Timer1 and reset control reg.
CLR TMR2 ; Clear contents of the timer register
MOV #0XFFFF,W0;Load the Period register
MOV w0, PR2 ; with the value 0x8CFF
;BSET IPC0, #T1IP0 ; Setup Timer1 interrupt for
;BCLR IPC0, #T1IP1 ; desired priority level
;BCLR IPC0, #T1IP2 ; (this example assigns level 1 priority)
;BCLR IFS0, #T1IF ; Clear the Timer1 interrupt status flag
;BSET IEC0, #T1IE ; Enable Timer1 interrupts
MOV #0x8016, W0 ; Start Timer1 with prescaler settings at
; 1:1 and clock source set to the external
MOV w0, T2CON ; clock in the synchronous mode
; Example code for Timer1 ISR
BCLR T2CON, #TCKPS0
BCLR T2CON, #TCKPS1
RETURN

TIMER32BIT:
CLR T2CON ; Stops any 16/32-bit Timer2 operation
CLR T3CON ; Stops any 16-bit Timer3 operation
CLR TMR3 ; Clear contents of the Timer3 timer register
CLR TMR2 ; Clear contents of the Timer2 timer register
MOV #0xF, w0 ; Load the Period Register 3
MOV w0, PR3 ; with the value 0xFFFF
MOV #0X4240,W0
MOV w0, PR2 ; Load the Period Register2 with value 0xFFFF
BSET IPC1, #T3IP0 ; Setup Timer3 interrupt for
BCLR IPC1, #T3IP1 ; desired priority level
BCLR IPC1, #T3IP2 ; (this example assigns level 1 priority)
BCLR IFS0, #T3IF ; Clear the Timer3 interrupt status flag
BSET IEC0, #T3IE ; Enable Timer3 interrupts
BSET T2CON, #T32 ; Enable 32-bit Timer operation
BSET T2CON, #TON ; Start 32-bit timer with prescaler
BSET T2CON, #TCKPS0 ;
; settings at 1:1 and clock source set to
; the internal instruction cycle
RETURN

;------------------------------------------------------------------------------
;Chuong trinh con khoi tao cac cong I/O, de noi voi LED
;------------------------------------------------------------------------------
Init_PORTE:
mov #0b1111110011111111,W0 ;LED noi vao cong E (RE0 .. RE5)
mov W0, HUONGE
MOV W0,DATAE
return

;------------------------------------------------------------------------------
Init_PORTG:
mov #0b1111111111110000,W0 ;LED noi vao cong E (RE0 .. RE5)
mov W0, HUONGG
MOV W0,DATAG
return

;---------------------------------------------------------------------------------
;Chuong trinh con khoi tao cac cong I/O, de xuat cac tin hieu PWM, va doc tin
;hieu dieu chinh cua bien tro tai AN0
;-------------------------------------------------------------------------------
Init_PORTb:
mov #0B0000111111111111, W0 ;Chan RB0 la ngo vao analog AN0, cac chan khac la
mov W0, HUONGB ;ngo ra
return

Init_PORTC:
mov #0XFFFF, W0 ;Chan RCla ngo vao analog
mov W0, HUONGC ;ngo ra
return

;---------------------------------------------------
Init_PORTD:
mov #0X0F00, W0
mov W0, HUONGD ;ngo ra
MOV W0,DATAD
return

Init_PORTF:
mov #0XFF00, W0
mov W0, HUONGF ;ngo ra
MOV W0,DATAF
return

;-------------------------------------------------------------------
init_adc:
mov #0XFFFE, W0 ;Cac chan khac la digital, chan AN0 la analog
mov W0, ADPCFG
mov #0x00E0, W0 ;Module ADC cham dut lay mau va kich hoat chuyen doi
mov W0, ADCON1

clr ADCON2

clr ADCHS ;Kenh 0 doc tin hieu giua AN0 va AVss
clr ADCSSL ;Khong quet cac ngo vao

mov #0x01F1F, W0 ;Dung 31 TAD cho lay mau, xung clock cua he thong,
mov W0, ADCON3 ;TAD = 2xTCY = 250 ns
; bclr IFS0, #ADIF ;Xoa co ngat ADC
; BCLR IFS0,#ADIF
; bset IEC0, #ADIE ;Cho phep ngat ADC
bset ADCON1, #ASAM ;Khoi dong che do tu dong lay mau
bset ADCON1, #ADON ;Bat module ADC
BSET ADCON1,#SAMP
return
;-------------------------------------------------------------------------------
;---------------------------------------------------------------------------------
init_pwm_motor:
CLR PTCON
BSET PTCON,#PTMOD1 ;

MOV #0b11111111,W0
MOV W0,PWMCON1
BCLR PWMCON1,#PEN3L ; NGO VAO CHAN PWM3H

CLR PWMCON2
BSET PWMCON2,#IUE ; ENABALE LOAD PDC1,2,3 NEW
BSET PWMCON2,#OSYNC
; Bclr PWMCON2,#UDIS ; = 1 PTPER NOT UPDATA
;393 ; TAN SO 20KHz =600
MOV #500,W0 ;700
MOV W0,PTPER

CLR FLTACON
MOV #0b0011111100000000,W0
MOV W0,OVDCON
CLR OVDCON

MOV #0,W0 ; 245
MOV W0,DPWM1
MOV W0,DPWM2
MOV W0,DPWM3

MOV #0B1111110000111111,W0 ;stop
MOV W0,DTCON1

MOV #0b0011111100000000,W0 ;run
MOV W0,OVDCON

BSET PTCON,#PTEN

return
;9999999999999999999999999999999999999999999999999 999999
UNIt_QEI:
MOV #0,W0
MOV W0,QEICON
BSET QEICON,#QEIM2
BSET QEICON,#QEIM1 ; X2
BSET QEICON,#QEIM0
BSET QEICON,#SWPAB

MOV #0XFFFF,W0
MOV W0,MAXCNT

RETURN

;************************************************* *****
THONGSO_ENCODER:
BTSS NHAN_ENCODER,#0
BCLR QEICON,#QEIM1 ; X2
BTSC NHAN_ENCODER,#0
BSET QEICON,#QEIM1 ; X2
RETURN

;9999999999999999999999999999999999999999999999999

HIEUCHINH_EEROM:
MOV #10,W2
MOV BIENTU,W0
MOV #10000,W1
SUB W1,W0,W3 ;<
BRA C,BT
MOV W2,BIENTU
BT:

MOV #10,W2
MOV BIENMAU,W0
MOV #10000,W1
SUB W1,W0,W3 ;<
BRA C,BM
MOV W2,BIENMAU
BM:

MOV #1100,W2
MOV GIAN,W0
MOV #10000,W1
SUB W1,W0,W3 ;<
BRA C,BG
MOV W2,GIAN
BG:
MOV #1,W2
MOV NHAN_ENCODER,W0
MOV #10000,W1
SUB W1,W0,W3 ;<
BRA C,BN
MOV W2,NHAN_ENCODER
BN:

MOV #1000,W2
MOV GIAN_D,W0
MOV #10000,W1
SUB W1,W0,W3 ;<
BRA C,BD
MOV W2,GIAN_D
BD:

MOV #50,W2
MOV GIAN_I,W0
MOV #2000,W1
SUB W1,W0,W3 ;<
BRA C,BI
MOV W2,GIAN_I
BI:

RETURN

.END

**mtuankct** · 20-03-2009, 12:01 PM

theo mình bạn sửa dòng này config __FGS, CODE_PROT_ON thành config __FGS, CODE_PROT_OFF

**thanhdai** · 20-03-2009, 06:38 PM

Trích:

Nguyên văn bởi mtuankct

theo mình bạn sửa dòng này config __FGS, CODE_PROT_ON thành config __FGS, CODE_PROT_OFF

cái này em cũng đã làm rồi. không tác dụng gì hết.
dòng này có tác dụng khóa chương trình không cho người khác đọc chương trình của chip thôi.

20-03-2009, 12:01 PM	#2
mtuankct Đệ tử 6 túi Tham gia ngày: Apr 2006 Bài gửi: 125 :	theo mình bạn sửa dòng này config __FGS, CODE_PROT_ON thành config __FGS, CODE_PROT_OFF