td_bien.asm

Robot control assembly program.

	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_RAT_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_TRAI
;--CHAY THANG 3 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_RAT_CHAM
	CALL	CHAY_THANG

;--CUA TRAI
	CALL	CUA_TRAI
	CALL	PHA_DIEM
		
	JB	NUT_TT1,$
	MOV	BYTE_LED_THAP,#88
	CALL	DELAY1S
	JMP	START

;=========CHUONH TRINH 1=========
CHUONG_TRINH_PHAI_CP1:	
;--PHA O 2 DIEM PHIA DOI DIEN DOI PHUONG
;--RA VUNG XUAT PHAT
	CALL	XUAT_PHAT_PHAI
;--CHAY THANG 8 VACH
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	MOV	DEM_VACH,#6
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_RAT_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 1 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	CALL	GHI_O_1_DIEM
	CALL	CUA_TRAI_90
;--CHAY THANG 2 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 4 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	MOV	DEM_VACH,#1
	CALL	VAN_TOC_TB
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_RAT_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 7 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	MOV	DEM_VACH,#1
	CALL	VAN_TOC_TB
	CALL	CHAY_THANG

	MOV	DEM_VACH,#4
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_RAT_CHAM
	CALL	CHAY_THANG

;--CUA TRAI
	CALL	CUA_TRAI
;--CHAY THANG 2 VACH
	CALL	PHA_DIEM

	JB	NUT_TT1,$
	MOV	BYTE_LED_THAP,#88
	CALL	DELAY1S
	JMP	START
;=========CHUONH TRINH 1=========
CHUONG_TRINH_PHAI_CP2:	
;--PHIA THAP 
;--RA VUNG XUAT PHAT
	CALL	XUAT_PHAT_PHAI
;--CHAY THANG 8 VACH
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	MOV	DEM_VACH,#6
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_RAT_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 1 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	CALL	GHI_O_1_DIEM
	CALL	CUA_TRAI_90
;--CHAY THANG 2 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 4 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	MOV	DEM_VACH,#1
	CALL	VAN_TOC_TB
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_RAT_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 4 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	MOV	DEM_VACH,#1
	CALL	VAN_TOC_TB
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_RAT_CHAM
	CALL	CHAY_THANG

;--CUA TRAI
	CALL	CUA_TRAI
;--CHAY THANG 3 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_TB
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

	CALL	CUA_PHAI

	CALL	PHA_DIEM

	JB	NUT_TT1,$
	MOV	BYTE_LED_THAP,#88
	CALL	DELAY1S
	JMP	START

;=========CHUONH TRINH 1=========
CHUONG_TRINH_PHAI_CP3:	
;--PHA O 2 DIEM PHIA DOI DIEN DOI PHUONG
;--RA VUNG XUAT PHAT
	CALL	XUAT_PHAT_PHAI
;--CHAY THANG 8 VACH
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	MOV	DEM_VACH,#6
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 1 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	CALL	GHI_O_1_DIEM
	CALL	CUA_TRAI_90
;--CHAY THANG 2 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 4 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	MOV	DEM_VACH,#1
	CALL	VAN_TOC_TB
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 7 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	MOV	DEM_VACH,#1
	CALL	VAN_TOC_TB
	CALL	CHAY_THANG

	MOV	DEM_VACH,#4
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA TRAI
	CALL	CUA_TRAI
;--CHAY THANG 2 VACH
	CALL	PHA_DIEM

	JB	NUT_TT1,$
	MOV	BYTE_LED_THAP,#88
	CALL	DELAY1S
	JMP	START
;=========CHUONH TRINH 12=========
CHUONG_TRINH_PHAI_CP4:	
;--GHI O 2 DIEM DOI PHUONG PHIA DOI PHUONG,CHONG RETRY
;--RA VUNG XUAT PHAT
	CALL	XUAT_PHAT_PHAI
;--CHAY THANG 8 VACH
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#6
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 1 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	CALL	GHI_O_1_DIEM
	CALL	CUA_TRAI_90
;--CHAY THANG 2 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 5 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_TB
	CALL	CHAY_THANG

	MOV	DEM_VACH,#2
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG 9 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_TB
	CALL	CHAY_THANG

	MOV	DEM_VACH,#5
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#2
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA TRAI
	CALL	CUA_TRAI
;--CHAY THANG 5 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG
	
	MOV	DEM_VACH,#1
	CALL	VAN_TOC_TB
	CALL	CHAY_THANG

	MOV	DEM_VACH,#2
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#2
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG
	
;--CUA TRAI
	CALL	CUA_TRAI
;--CHAY THANG 2 VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_TB
	CALL	CHAY_THANG

;--CUA TRAI
	CALL	CUA_TRAI

	CALL	PHA_DIEM

	JB	NUT_TT1,$
	MOV	BYTE_LED_THAP,#88
	CALL	DELAY1S
	JMP	START
;=========CHUONH TRINH 1=========
CHUONG_TRINH_TRANG_CP1:	
;--GHI O 1 DIEM
;--RA VUNG XUAT PHAT
	CALL	XUAT_PHAT_PHAI
;--CHAY THANG 8 VACH	
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#6
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG  VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG

	CALL	GHI_O_1_DIEM
	CALL	HIEU_CHINH_DIEM

	MOV	ENCODER_THAP,#LOW(ENCODER_1_VACH/10)
	MOV	ENCODER_CAO,#HIGH(ENCODER_1_VACH/10)
	CALL	CHAY_LUI_ENCODER

	JB	NUT_TT1,$
	MOV	BYTE_LED_THAP,#88
	CALL	DELAY1S
	JMP	START
;=========CHUONH TRINH 1=========
CHUONG_TRINH_TRANG_CP2:	
;--GHI O 1 DIEM
;--RA VUNG XUAT PHAT
	CALL	XUAT_PHAT_PHAI
;--CHAY THANG 8 VACH	
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG_TIM_VACH_NGANG

	MOV	DEM_VACH,#6
	CALL	VAN_TOC_MAX
	CALL	CHAY_THANG

	MOV	DEM_VACH,#1
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG

;--CUA PHAI
	CALL	CUA_PHAI
;--CHAY THANG  VACH
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_TIM_VACH_NGANG

	CALL	GHI_O_1_DIEM
	CALL	HIEU_CHINH_DIEM

	MOV	ENCODER_THAP,#LOW(ENCODER_1_VACH/10)
	MOV	ENCODER_CAO,#HIGH(ENCODER_1_VACH/10)
	CALL	CHAY_LUI_ENCODER

	JB	NUT_TT1,$
	MOV	BYTE_LED_THAP,#88
	CALL	DELAY1S
	JMP	START

;======================= VAO VUNG BANG TAY ==========
CAN_BT:	
;--BO CAN BT
	MOV	BANH_TRAI,#160
	MOV	BANH_PHAI,#255
	MOV	ENCODER_THAP,#LOW(ENCODER_1_VACH*8)
	MOV	ENCODER_CAO,#HIGH(ENCODER_1_VACH*8)
	SETB	ACT_ENCODER_PHAI
LOOP_BO_CAN_BT:
	JNB	ACT_ENCODER_PHAI,THOAT_LOOP_BO_CAN_BT
	MOV	BYTE_LED_THAP,ENCODER_THAP	;<---
	MOV	BYTE_LED_CAO,ENCODER_CAO

	MOV	C,CTHT_TRAI
	ANL	C,CTHT_PHAI
	JC	LOOP_BO_CAN_BT
	SETB	BIT_ERROR_CAN_BT

	JMP	LOOP_BO_CAN_BT
THOAT_LOOP_BO_CAN_BT:

;--CHAY THANG
	MOV	BANH_TRAI,#240
	MOV	BANH_PHAI,#255
	MOV	ENCODER_THAP,#LOW(ENCODER_1_VACH*47/10)
	MOV	ENCODER_CAO,#HIGH(ENCODER_1_VACH*47/10)
	SETB	ACT_ENCODER_PHAI
LOOP_CHAY_THANG_CAN_BT:
	JNB	ACT_ENCODER_PHAI,THOAT_LOOP_CHAY_THANG_CAN_BT
	MOV	BYTE_LED_THAP,ENCODER_THAP	;<---
	MOV	BYTE_LED_CAO,ENCODER_CAO

	MOV	C,CTHT_TRAI
	ANL	C,CTHT_PHAI
	JC	LOOP_CHAY_THANG_CAN_BT
	SETB	BIT_ERROR_CAN_BT

	JMP	LOOP_CHAY_THANG_CAN_BT
THOAT_LOOP_CHAY_THANG_CAN_BT:	

;--
	JNB	BIT_ERROR_CAN_BT,CUA_PHAI_CAN_BT
	MOV	R2,#5
LOOP_ERROR_CAN_BT:
	MOV	BYTE_LED_THAP,#99	;<----
	MOV	BANH_TRAI,#255
	MOV	BANH_PHAI,#255
	MOV	ENCODER_THAP,#LOW(ENCODER_1_VACH)
	MOV	ENCODER_CAO,#HIGH(ENCODER_1_VACH)
	SETB	ACT_ENCODER_PHAI
	JB	ACT_ENCODER_PHAI,$
	CALL	DUNG

	MOV	C,CTHT_TRAI
	ANL	C,CTHT_PHAI
	JC	THOAT_ERROR_CAN_BT
	DJNZ	R2,SUA_ERROR_CAN_BT
	JMP	THOAT_ERROR_CAN_BT
SUA_ERROR_CAN_BT:
	MOV	ENCODER_THAP,#LOW(ENCODER_1_VACH/8)
	MOV	ENCODER_CAO,#HIGH(ENCODER_1_VACH/8)
	CALL	CHAY_LUI_ENCODER
	CALL	DELAY1S
	JMP	LOOP_ERROR_CAN_BT

THOAT_ERROR_CAN_BT:
	CALL	CUA_PHAI_ENCODER
	SJMP	$

CUA_PHAI_CAN_BT:
;--CUA PHAI
	CALL	HAM_TOC
	CALL	HAM_TOC
	CALL	DELAY200MS
	CALL	CUA_PHAI_ENCODER

THOAT_CAN_BT:
	RET
;======================= GHI_O_1_DIEM ===============
GHI_O_1_DIEM:	
	MOV	ENCODER_CAO,#HIGH(ENCODER_1_VACH*50/100)
	MOV	ENCODER_THAP,#LOW(ENCODER_1_VACH*50/100)
	CALL	VAN_TOC_CHAM
	CALL	CHAY_THANG_ENCODER
	CALL	HAM_TOC

	SETB	MOTOR_DAY_DAO1
	SETB	MOTOR_DAY_DAO2
	MOV	BANH_TRAI,#0
	MOV	BANH_PHAI,#0
	CALL	DELAY50MS

	MOV	MOTOR_DAY1,#255
	MOV	MOTOR_DAY2,#255
	CALL	DELAY1S

	MOV	BYTE_DELAY_THAP,#LOW(200)
	MOV	BYTE_DELAY_CAO,#HIGH(200)
	INC	BYTE_DELAY_CAO
	SETB	TR0
	MOV	MOTOR_DAY1,#100
	MOV	MOTOR_DAY2,#100
	
LOOP_TR0_CTHT_TRUOC_GHI_1_DIEM:
	MOV	C,TR0
	ANL	C,CTHT_TRUOC
	JC	LOOP_TR0_CTHT_TRUOC_GHI_1_DIEM

	SETB	BIT_GHI_DIEM
	MOV	MOTOR_DAY1,#0
	MOV	MOTOR_DAY2,#0
	CLR	MOTOR_DAY_DAO1
	CLR	MOTOR_DAY_DAO2
	CALL	DELAY200MS	;CHO QUA ON DINH
	MOV	MOTOR_DAY1,#255
	MOV	MOTOR_DAY2,#255
	CALL	DELAY1S
	MOV	MOTOR_DAY1,#255
	MOV	MOTOR_DAY2,#255
	CALL	DELAY1S
	MOV	MOTOR_DAY1,#50
	MOV	MOTOR_DAY2,#50

	RET
;======================= GHI DIEM =============================
GHI_DIEM:			;DO DUONG CHO DEN KHI CHAM CTHT
	MOV	BYTE_DELAY_THAP,#LOW(200)	;TIME OUT 1S
	MOV	BYTE_DELAY_CAO,#HIGH(200)
	INC	BYTE_DELAY_CAO
	SETB	TR0
	CALL	VAN_TOC_HC
HIEU_CHINH_GHI_DIEM:	
	CALL	KT_VUNG_DO_DUONG
	CALL	DK_DO_DUONG
	MOV	C,CTHT_TRAI
	ANL	C,CTHT_PHAI
	JC	KT_TIME_OUT_HC 
	CALL	BD_GHI_DIEM
	JMP	THOAT_GHI_DIEM
KT_TIME_OUT_HC:
	JB	TR0,HIEU_CHINH_GHI_DIEM
	CALL	FIX_ERROR_SCORE
THOAT_GHI_DIEM:	
	RET

;---------------- ERROR SCORE POINT ------------------------------
FIX_ERROR_SCORE:
	CALL	GHI_DIEM_LAI
	JB	BIT_GHI_DIEM,THOAT_FIX_ERROR_SCORE

	CALL	DUNG
	CALL	DELAY30S	;TIME OUT
	CALL	BD_GHI_DIEM
	CALL	GIU_DIEM

THOAT_FIX_ERROR_SCORE:	
	RET
;-----------------------------------------------
BD_GHI_DIEM:	
	MOV	MOTOR_DAY1,#255
	MOV	MOTOR_DAY2,#255
	MOV	BANH_TRAI,#30
	MOV	BANH_PHAI,#30
	CALL	DELAY1S
	CALL	DELAY500MS
	MOV	MOTOR_DAY1,#100
	MOV	MOTOR_DAY2,#100

	MOV	BYTE_DELAY_THAP,#LOW(200)
	MOV	BYTE_DELAY_CAO,#HIGH(200)
	INC	BYTE_DELAY_CAO
	SETB	TR0

LOOP_TR0_CTHT_TRUOC_GHI_DIEM:
	MOV	C,TR0
	ANL	C,CTHT_TRUOC
	JC	LOOP_TR0_CTHT_TRUOC_GHI_DIEM

	SETB	BIT_GHI_DIEM
;GIU COT CHO QUA ON DINH
	MOV	MOTOR_DAY1,#0
	MOV	MOTOR_DAY2,#0
	MOV	BANH_TRAI,#30
	MOV	BANH_PHAI,#30
	CALL	DELAY500MS

HIEU_CHINH_DIEM:
	MOV	BANH_TRAI,#0
	MOV	BANH_PHAI,#0
	CLR	BANH_TRAI_DAO
	CLR	BANH_PHAI_DAO
	CALL	DELAY50MS

	MOV	BANH_TRAI,#100
	MOV	BANH_PHAI,#100
	MOV	ENCODER_CAO,#HIGH(ENCODER_1_VACH/10)
	MOV	ENCODER_THAP,#LOW(ENCODER_1_VACH/10)
	SETB	ACT_ENCODER_PHAI
	JB	ACT_ENCODER_PHAI,$

	MOV	BANH_TRAI,#0
	MOV	BANH_PHAI,#0
	SETB	BANH_TRAI_DAO
	SETB	BANH_PHAI_DAO
	CALL	DELAY50MS

	MOV	BANH_TRAI,#80
	MOV	BANH_PHAI,#80
	CALL	DELAY500MS
	MOV	BANH_TRAI,#30
	MOV	BANH_PHAI,#30
		
	RET
;============== GHI DIEM LAI =============
GHI_DIEM_LAI:
	MOV	R2,#2
LOOP_HIEU_CHINH_GHI_DIEM_ERROR:
;LUI
	MOV	BANH_TRAI,#0