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
vBulletin® v3.8.11, Copyright ©2000-2024, vBulletin Solutions Inc.