Chat
Danh mục
Code Robocon tự động

Code Robocon tự động

Số lượng:
Thêm vào giỏ
Code Robocon tự động đã được thêm vào giỏ hàng


Hôm nay Chip chia sẻ mọi người tham khảo Code chương trình Robo tự động dò đường.Lập trình bằng CodeVision AVR.Chip cũng đang học món này khó hiểu quá đi mất.hix hix.Bác nào Pro thì chỉ dùm nha.
Update code Robo tự động lập trình bằng C++:

;CodeVisionAVR C Compiler V1.24.8d Professional
;(C) Copyright 1998-2006 Pavel Haiduc, HP InfoTech s.r.l.
;http://www.hpinfotech.com

;Chip type : ATmega16L
;Program type : Application
;Clock frequency : 16.000000 MHz
;Memory model : Small
;Optimize for : Size
;(s)printf features : int, width
;(s)scanf features : int, width
;External SRAM size : 0
;Data Stack size : 256 byte(s)
;Heap size : 0 byte(s)
;Promote char to int : No
;char is unsigned : Yes
;8 bit enums : Yes
;Word align FLASH struct: No
;Enhanced core instructions : On
;Automatic register allocation : On

#pragma AVRPART ADMIN PART_NAME ATmega16L
#pragma AVRPART MEMORY PROG_FLASH 16384
#pragma AVRPART MEMORY EEPROM 512
#pragma AVRPART MEMORY INT_SRAM SIZE 1024
#pragma AVRPART MEMORY INT_SRAM START_ADDR 0x60

.EQU UDRE=0x5
.EQU RXC=0x7
.EQU USR=0xB
.EQU UDR=0xC
.EQU SPSR=0xE
.EQU SPDR=0xF
.EQU EERE=0x0
.EQU EEWE=0x1
.EQU EEMWE=0x2
.EQU EECR=0x1C
.EQU EEDR=0x1D
.EQU EEARL=0x1E
.EQU EEARH=0x1F
.EQU WDTCR=0x21
.EQU MCUCR=0x35
.EQU GICR=0x3B
.EQU SPL=0x3D
.EQU SPH=0x3E
.EQU SREG=0x3F

.DEF R0X0=R0
.DEF R0X1=R1
.DEF R0X2=R2
.DEF R0X3=R3
.DEF R0X4=R4
.DEF R0X5=R5
.DEF R0X6=R6
.DEF R0X7=R7
.DEF R0X8=R8
.DEF R0X9=R9
.DEF R0XA=R10
.DEF R0XB=R11
.DEF R0XC=R12
.DEF R0XD=R13
.DEF R0XE=R14
.DEF R0XF=R15
.DEF R0X10=R16
.DEF R0X11=R17
.DEF R0X12=R18
.DEF R0X13=R19
.DEF R0X14=R20
.DEF R0X15=R21
.DEF R0X16=R22
.DEF R0X17=R23
.DEF R0X18=R24
.DEF R0X19=R25
.DEF R0X1A=R26
.DEF R0X1B=R27
.DEF R0X1C=R28
.DEF R0X1D=R29
.DEF R0X1E=R30
.DEF R0X1F=R31

.EQU __se_bit=0x40
.EQU __sm_mask=0xB0
.EQU __sm_adc_noise_red=0x10
.EQU __sm_powerdown=0x20
.EQU __sm_powersave=0x30
.EQU __sm_standby=0xA0
.EQU __sm_ext_standby=0xB0

.MACRO __CPD1N
CPI R30,LOW(@0)
LDI R26,HIGH(@0)
CPC R31,R26
LDI R26,BYTE3(@0)
CPC R22,R26
LDI R26,BYTE4(@0)
CPC R23,R26
.ENDM

.MACRO __CPD2N
CPI R26,LOW(@0)
LDI R30,HIGH(@0)
CPC R27,R30
LDI R30,BYTE3(@0)
CPC R24,R30
LDI R30,BYTE4(@0)
CPC R25,R30
.ENDM

.MACRO __CPWRR
CP R@0,R@2
CPC R@1,R@3
.ENDM

.MACRO __CPWRN
CPI R@0,LOW(@2)
LDI R30,HIGH(@2)
CPC R@1,R30
.ENDM

.MACRO __ADDB1MN
SUBI R30,LOW(-@0-(@1))
.ENDM
.MACRO __ADDB2MN
SUBI R26,LOW(-@0-(@1))
.ENDM
.MACRO __ADDW1MN
SUBI R30,LOW(-@0-(@1))
SBCI R31,HIGH(-@0-(@1))
.ENDM
.MACRO __ADDW2MN
SUBI R26,LOW(-@0-(@1))
SBCI R27,HIGH(-@0-(@1))
.ENDM
.MACRO __ADDW1FN
SUBI R30,LOW(-2*@0-(@1))
SBCI R31,HIGH(-2*@0-(@1))
.ENDM
.MACRO __ADDD1FN
SUBI R30,LOW(-2*@0-(@1))
SBCI R31,HIGH(-2*@0-(@1))
SBCI R22,BYTE3(-2*@0-(@1))
.ENDM
.MACRO __ADDD1N
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
SBCI R22,BYTE3(-@0)
SBCI R23,BYTE4(-@0)
.ENDM

.MACRO __ADDD2N
SUBI R26,LOW(-@0)
SBCI R27,HIGH(-@0)
SBCI R24,BYTE3(-@0)
SBCI R25,BYTE4(-@0)
.ENDM

.MACRO __SUBD1N
SUBI R30,LOW(@0)
SBCI R31,HIGH(@0)
SBCI R22,BYTE3(@0)
SBCI R23,BYTE4(@0)
.ENDM

.MACRO __SUBD2N
SUBI R26,LOW(@0)
SBCI R27,HIGH(@0)
SBCI R24,BYTE3(@0)
SBCI R25,BYTE4(@0)
.ENDM

.MACRO __ANDBMNN
LDS R30,@0+@1
ANDI R30,LOW(@2)
STS @0+@1,R30
.ENDM

.MACRO __ANDWMNN
LDS R30,@0+@1
ANDI R30,LOW(@2)
STS @0+@1,R30
LDS R30,@0+@1+1
ANDI R30,HIGH(@2)
STS @0+@1+1,R30
.ENDM

.MACRO __ANDD1N
ANDI R30,LOW(@0)
ANDI R31,HIGH(@0)
ANDI R22,BYTE3(@0)
ANDI R23,BYTE4(@0)
.ENDM

.MACRO __ORBMNN
LDS R30,@0+@1
ORI R30,LOW(@2)
STS @0+@1,R30
.ENDM

.MACRO __ORWMNN
LDS R30,@0+@1
ORI R30,LOW(@2)
STS @0+@1,R30
LDS R30,@0+@1+1
ORI R30,HIGH(@2)
STS @0+@1+1,R30
.ENDM

.MACRO __ORD1N
ORI R30,LOW(@0)
ORI R31,HIGH(@0)
ORI R22,BYTE3(@0)
ORI R23,BYTE4(@0)
.ENDM

.MACRO __DELAY_USB
LDI R24,LOW(@0)
__DELAY_USB_LOOP:
DEC R24
BRNE __DELAY_USB_LOOP
.ENDM

.MACRO __DELAY_USW
LDI R24,LOW(@0)
LDI R25,HIGH(@0)
__DELAY_USW_LOOP:
SBIW R24,1
BRNE __DELAY_USW_LOOP
.ENDM

.MACRO __CLRD1S
LDI R30,0
STD Y+@0,R30
STD Y+@0+1,R30
STD Y+@0+2,R30
STD Y+@0+3,R30
.ENDM

.MACRO __GETD1S
LDD R30,Y+@0
LDD R31,Y+@0+1
LDD R22,Y+@0+2
LDD R23,Y+@0+3
.ENDM

.MACRO __PUTD1S
STD Y+@0,R30
STD Y+@0+1,R31
STD Y+@0+2,R22
STD Y+@0+3,R23
.ENDM

.MACRO __PUTD2S
STD Y+@0,R26
STD Y+@0+1,R27
STD Y+@0+2,R24
STD Y+@0+3,R25
.ENDM

.MACRO __POINTB1MN
LDI R30,LOW(@0+@1)
.ENDM

.MACRO __POINTW1MN
LDI R30,LOW(@0+@1)
LDI R31,HIGH(@0+@1)
.ENDM

.MACRO __POINTD1M
LDI R30,LOW(@0)
LDI R31,HIGH(@0)
LDI R22,BYTE3(@0)
.ENDM

.MACRO __POINTW1FN
LDI R30,LOW(2*@0+@1)
LDI R31,HIGH(2*@0+@1)
.ENDM

.MACRO __POINTD1FN
LDI R30,LOW(2*@0+@1)
LDI R31,HIGH(2*@0+@1)
LDI R22,BYTE3(2*@0+@1)
.ENDM

.MACRO __POINTB2MN
LDI R26,LOW(@0+@1)
.ENDM

.MACRO __POINTW2MN
LDI R26,LOW(@0+@1)
LDI R27,HIGH(@0+@1)
.ENDM

.MACRO __POINTBRM
LDI R@0,LOW(@1)
.ENDM

.MACRO __POINTWRM
LDI R@0,LOW(@2)
LDI R@1,HIGH(@2)
.ENDM

.MACRO __POINTBRMN
LDI R@0,LOW(@1+@2)
.ENDM

.MACRO __POINTWRMN
LDI R@0,LOW(@2+@3)
LDI R@1,HIGH(@2+@3)
.ENDM

.MACRO __POINTWRFN
LDI R@0,LOW(@2*2+@3)
LDI R@1,HIGH(@2*2+@3)
.ENDM

.MACRO __GETD1N
LDI R30,LOW(@0)
LDI R31,HIGH(@0)
LDI R22,BYTE3(@0)
LDI R23,BYTE4(@0)
.ENDM

.MACRO __GETD2N
LDI R26,LOW(@0)
LDI R27,HIGH(@0)
LDI R24,BYTE3(@0)
LDI R25,BYTE4(@0)
.ENDM

.MACRO __GETD2S
LDD R26,Y+@0
LDD R27,Y+@0+1
LDD R24,Y+@0+2
LDD R25,Y+@0+3
.ENDM

.MACRO __GETB1MN
LDS R30,@0+@1
.ENDM

.MACRO __GETB1HMN
LDS R31,@0+@1
.ENDM

.MACRO __GETW1MN
LDS R30,@0+@1
LDS R31,@0+@1+1
.ENDM

.MACRO __GETD1MN
LDS R30,@0+@1
LDS R31,@0+@1+1
LDS R22,@0+@1+2
LDS R23,@0+@1+3
.ENDM

.MACRO __GETBRMN
LDS R@0,@1+@2
.ENDM

.MACRO __GETWRMN
LDS R@0,@2+@3
LDS R@1,@2+@3+1
.ENDM

.MACRO __GETWRZ
LDD R@0,Z+@2
LDD R@1,Z+@2+1
.ENDM

.MACRO __GETD2Z
LDD R26,Z+@0
LDD R27,Z+@0+1
LDD R24,Z+@0+2
LDD R25,Z+@0+3
.ENDM

.MACRO __GETB2MN
LDS R26,@0+@1
.ENDM

.MACRO __GETW2MN
LDS R26,@0+@1
LDS R27,@0+@1+1
.ENDM

.MACRO __GETD2MN
LDS R26,@0+@1
LDS R27,@0+@1+1
LDS R24,@0+@1+2
LDS R25,@0+@1+3
.ENDM

.MACRO __PUTB1MN
STS @0+@1,R30
.ENDM

.MACRO __PUTW1MN
STS @0+@1,R30
STS @0+@1+1,R31
.ENDM

.MACRO __PUTD1MN
STS @0+@1,R30
STS @0+@1+1,R31
STS @0+@1+2,R22
STS @0+@1+3,R23
.ENDM

.MACRO __PUTDZ2
STD Z+@0,R26
STD Z+@0+1,R27
STD Z+@0+2,R24
STD Z+@0+3,R25
.ENDM

.MACRO __PUTBMRN
STS @0+@1,R@2
.ENDM

.MACRO __PUTWMRN
STS @0+@1,R@2
STS @0+@1+1,R@3
.ENDM

.MACRO __PUTBZR
STD Z+@1,R@0
.ENDM

.MACRO __PUTWZR
STD Z+@2,R@0
STD Z+@2+1,R@1
.ENDM

.MACRO __GETW1R
MOV R30,R@0
MOV R31,R@1
.ENDM

.MACRO __GETW2R
MOV R26,R@0
MOV R27,R@1
.ENDM

.MACRO __GETWRN
LDI R@0,LOW(@2)
LDI R@1,HIGH(@2)
.ENDM

.MACRO __PUTW1R
MOV R@0,R30
MOV R@1,R31
.ENDM

.MACRO __PUTW2R
MOV R@0,R26
MOV R@1,R27
.ENDM

.MACRO __ADDWRN
SUBI R@0,LOW(-@2)
SBCI R@1,HIGH(-@2)
.ENDM

.MACRO __ADDWRR
ADD R@0,R@2
ADC R@1,R@3
.ENDM

.MACRO __SUBWRN
SUBI R@0,LOW(@2)
SBCI R@1,HIGH(@2)
.ENDM

.MACRO __SUBWRR
SUB R@0,R@2
SBC R@1,R@3
.ENDM

.MACRO __ANDWRN
ANDI R@0,LOW(@2)
ANDI R@1,HIGH(@2)
.ENDM

.MACRO __ANDWRR
AND R@0,R@2
AND R@1,R@3
.ENDM

.MACRO __ORWRN
ORI R@0,LOW(@2)
ORI R@1,HIGH(@2)
.ENDM

.MACRO __ORWRR
OR R@0,R@2
OR R@1,R@3
.ENDM

.MACRO __EORWRR
EOR R@0,R@2
EOR R@1,R@3
.ENDM

.MACRO __GETWRS
LDD R@0,Y+@2
LDD R@1,Y+@2+1
.ENDM

.MACRO __PUTWSR
STD Y+@2,R@0
STD Y+@2+1,R@1
.ENDM

.MACRO __MOVEWRR
MOV R@0,R@2
MOV R@1,R@3
.ENDM

.MACRO __INWR
IN R@0,@2
IN R@1,@2+1
.ENDM

.MACRO __OUTWR
OUT @2+1,R@1
OUT @2,R@0
.ENDM

.MACRO __CALL1MN
LDS R30,@0+@1
LDS R31,@0+@1+1
ICALL
.ENDM

.MACRO __CALL1FN
LDI R30,LOW(2*@0+@1)
LDI R31,HIGH(2*@0+@1)
CALL __GETW1PF
ICALL
.ENDM

.MACRO __CALL2EN
LDI R26,LOW(@0+@1)
LDI R27,HIGH(@0+@1)
CALL __EEPROMRDW
ICALL
.ENDM

.MACRO __GETW1STACK
IN R26,SPL
IN R27,SPH
ADIW R26,@0+1
LD R30,X+
LD R31,X
.ENDM

.MACRO __NBST
BST R@0,@1
IN R30,SREG
LDI R31,0x40
EOR R30,R31
OUT SREG,R30
.ENDM


.MACRO __PUTB1SN
LDD R26,Y+@0
LDD R27,Y+@0+1
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
ST X,R30
.ENDM

.MACRO __PUTW1SN
LDD R26,Y+@0
LDD R27,Y+@0+1
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
ST X+,R30
ST X,R31
.ENDM

.MACRO __PUTD1SN
LDD R26,Y+@0
LDD R27,Y+@0+1
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
CALL __PUTDP1
.ENDM

.MACRO __PUTB1SNS
LDD R26,Y+@0
LDD R27,Y+@0+1
ADIW R26,@1
ST X,R30
.ENDM

.MACRO __PUTW1SNS
LDD R26,Y+@0
LDD R27,Y+@0+1
ADIW R26,@1
ST X+,R30
ST X,R31
.ENDM

.MACRO __PUTD1SNS
LDD R26,Y+@0
LDD R27,Y+@0+1
ADIW R26,@1
CALL __PUTDP1
.ENDM

.MACRO __PUTB1PMN
LDS R26,@0
LDS R27,@0+1
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
ST X,R30
.ENDM

.MACRO __PUTW1PMN
LDS R26,@0
LDS R27,@0+1
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
ST X+,R30
ST X,R31
.ENDM

.MACRO __PUTD1PMN
LDS R26,@0
LDS R27,@0+1
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
CALL __PUTDP1
.ENDM

.MACRO __PUTB1PMNS
LDS R26,@0
LDS R27,@0+1
ADIW R26,@1
ST X,R30
.ENDM

.MACRO __PUTW1PMNS
LDS R26,@0
LDS R27,@0+1
ADIW R26,@1
ST X+,R30
ST X,R31
.ENDM

.MACRO __PUTD1PMNS
LDS R26,@0
LDS R27,@0+1
ADIW R26,@1
CALL __PUTDP1
.ENDM

.MACRO __PUTB1RN
MOVW R26,R@0
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
ST X,R30
.ENDM

.MACRO __PUTW1RN
MOVW R26,R@0
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
ST X+,R30
ST X,R31
.ENDM

.MACRO __PUTD1RN
MOVW R26,R@0
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
CALL __PUTDP1
.ENDM

.MACRO __PUTB1RNS
MOVW R26,R@0
ADIW R26,@1
ST X,R30
.ENDM

.MACRO __PUTW1RNS
MOVW R26,R@0
ADIW R26,@1
ST X+,R30
ST X,R31
.ENDM

.MACRO __PUTD1RNS
MOVW R26,R@0
ADIW R26,@1
CALL __PUTDP1
.ENDM

.MACRO __PUTB1RON
MOV R26,R@0
MOV R27,R@1
SUBI R26,LOW(-@2)
SBCI R27,HIGH(-@2)
ST X,R30
.ENDM

.MACRO __PUTW1RON
MOV R26,R@0
MOV R27,R@1
SUBI R26,LOW(-@2)
SBCI R27,HIGH(-@2)
ST X+,R30
ST X,R31
.ENDM

.MACRO __PUTD1RON
MOV R26,R@0
MOV R27,R@1
SUBI R26,LOW(-@2)
SBCI R27,HIGH(-@2)
CALL __PUTDP1
.ENDM

.MACRO __PUTB1RONS
MOV R26,R@0
MOV R27,R@1
ADIW R26,@2
ST X,R30
.ENDM

.MACRO __PUTW1RONS
MOV R26,R@0
MOV R27,R@1
ADIW R26,@2
ST X+,R30
ST X,R31
.ENDM

.MACRO __PUTD1RONS
MOV R26,R@0
MOV R27,R@1
ADIW R26,@2
CALL __PUTDP1
.ENDM


.MACRO __GETB1SX
MOVW R30,R28
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
LD R30,Z
.ENDM

.MACRO __GETB1HSX
MOVW R30,R28
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
LD R31,Z
.ENDM

.MACRO __GETW1SX
MOVW R30,R28
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
LD R0,Z+
LD R31,Z
MOV R30,R0
.ENDM

.MACRO __GETD1SX
MOVW R30,R28
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
LD R0,Z+
LD R1,Z+
LD R22,Z+
LD R23,Z
MOVW R30,R0
.ENDM

.MACRO __GETB2SX
MOVW R26,R28
SUBI R26,LOW(-@0)
SBCI R27,HIGH(-@0)
LD R26,X
.ENDM

.MACRO __GETW2SX
MOVW R26,R28
SUBI R26,LOW(-@0)
SBCI R27,HIGH(-@0)
LD R0,X+
LD R27,X
MOV R26,R0
.ENDM

.MACRO __GETD2SX
MOVW R26,R28
SUBI R26,LOW(-@0)
SBCI R27,HIGH(-@0)
LD R0,X+
LD R1,X+
LD R24,X+
LD R25,X
MOVW R26,R0
.ENDM

.MACRO __GETBRSX
MOVW R30,R28
SUBI R30,LOW(-@1)
SBCI R31,HIGH(-@1)
LD R@0,Z
.ENDM

.MACRO __GETWRSX
MOVW R30,R28
SUBI R30,LOW(-@2)
SBCI R31,HIGH(-@2)
LD R@0,Z+
LD R@1,Z
.ENDM

.MACRO __LSLW8SX
MOVW R30,R28
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
LD R31,Z
CLR R30
.ENDM

.MACRO __PUTB1SX
MOVW R26,R28
SUBI R26,LOW(-@0)
SBCI R27,HIGH(-@0)
ST X,R30
.ENDM

.MACRO __PUTW1SX
MOVW R26,R28
SUBI R26,LOW(-@0)
SBCI R27,HIGH(-@0)
ST X+,R30
ST X,R31
.ENDM

.MACRO __PUTD1SX
MOVW R26,R28
SUBI R26,LOW(-@0)
SBCI R27,HIGH(-@0)
ST X+,R30
ST X+,R31
ST X+,R22
ST X,R23
.ENDM

.MACRO __CLRW1SX
MOVW R30,R28
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
CLR R0
ST Z+,R0
ST Z,R0
.ENDM

.MACRO __CLRD1SX
MOVW R30,R28
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
CLR R0
ST Z+,R0
ST Z+,R0
ST Z+,R0
ST Z,R0
.ENDM

.MACRO __PUTB2SX
MOVW R30,R28
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
ST Z,R26
.ENDM

.MACRO __PUTW2SX
MOVW R30,R28
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
ST Z+,R26
ST Z,R27
.ENDM

.MACRO __PUTD2SX
MOVW R30,R28
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
ST Z+,R26
ST Z+,R27
ST Z+,R24
ST Z,R25
.ENDM

.MACRO __PUTBSRX
MOVW R30,R28
SUBI R30,LOW(-@0)
SBCI R31,HIGH(-@0)
ST Z,R@1
.ENDM

.MACRO __PUTWSRX
MOVW R30,R28
SUBI R30,LOW(-@2)
SBCI R31,HIGH(-@2)
ST Z+,R@0
ST Z,R@1
.ENDM

.MACRO __PUTB1SNX
MOVW R26,R28
SUBI R26,LOW(-@0)
SBCI R27,HIGH(-@0)
LD R0,X+
LD R27,X
MOV R26,R0
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
ST X,R30
.ENDM

.MACRO __PUTW1SNX
MOVW R26,R28
SUBI R26,LOW(-@0)
SBCI R27,HIGH(-@0)
LD R0,X+
LD R27,X
MOV R26,R0
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
ST X+,R30
ST X,R31
.ENDM

.MACRO __PUTD1SNX
MOVW R26,R28
SUBI R26,LOW(-@0)
SBCI R27,HIGH(-@0)
LD R0,X+
LD R27,X
MOV R26,R0
SUBI R26,LOW(-@1)
SBCI R27,HIGH(-@1)
ST X+,R30
ST X+,R31
ST X+,R22
ST X,R23
.ENDM

.MACRO __MULBRR
MULS R@0,R@1
MOVW R30,R0
.ENDM

.MACRO __MULBRRU
MUL R@0,R@1
MOVW R30,R0
.ENDM

.MACRO __MULBRR0
MULS R@0,R@1
.ENDM

.MACRO __MULBRRU0
MUL R@0,R@1
.ENDM

.MACRO __MULBNWRU
LDI R26,@2
MUL R26,R@0
MOVW R30,R0
MUL R26,R@1
ADD R31,R0
.ENDM

.CSEG
.ORG 0

.INCLUDE "doduong0.vec"
.INCLUDE "doduong0.inc"

__RESET:
CLI
CLR R30
OUT EECR,R30

;INTERRUPT VECTORS ARE PLACED
;AT THE START OF FLASH
LDI R31,1
OUT GICR,R31
OUT GICR,R30
OUT MCUCR,R30

;DISABLE WATCHDOG
LDI R31,0x18
OUT WDTCR,R31
OUT WDTCR,R30

;CLEAR R2-R14
LDI R24,13
LDI R26,2
CLR R27
__CLEAR_REG:
ST X+,R30
DEC R24
BRNE __CLEAR_REG

;CLEAR SRAM
LDI R24,LOW(0x400)
LDI R25,HIGH(0x400)
LDI R26,0x60
__CLEAR_SRAM:
ST X+,R30
SBIW R24,1
BRNE __CLEAR_SRAM

;GLOBAL VARIABLES INITIALIZATION
LDI R30,LOW(__GLOBAL_INI_TBL*2)
LDI R31,HIGH(__GLOBAL_INI_TBL*2)
__GLOBAL_INI_NEXT:
LPM R24,Z+
LPM R25,Z+
SBIW R24,0
BREQ __GLOBAL_INI_END
LPM R26,Z+
LPM R27,Z+
LPM R0,Z+
LPM R1,Z+
MOVW R22,R30
MOVW R30,R0
__GLOBAL_INI_LOOP:
LPM R0,Z+
ST X+,R0
SBIW R24,1
BRNE __GLOBAL_INI_LOOP
MOVW R30,R22
RJMP __GLOBAL_INI_NEXT
__GLOBAL_INI_END:

;STACK POINTER INITIALIZATION
LDI R30,LOW(0x45F)
OUT SPL,R30
LDI R30,HIGH(0x45F)
OUT SPH,R30

;DATA STACK POINTER INITIALIZATION
LDI R28,LOW(0x160)
LDI R29,HIGH(0x160)

JMP _main

.ESEG
.ORG 0

.DSEG
.ORG 0x160
; 1 //CHUONG TRINH DIEU KHIEN ROBOT TU DONG
; 2 //*****************************************************/
; 3 #include <mega16.h>
; 4
; 5 // Alphanumeric LCD Module functions
; 6 #asm
; 7 #endasm
; 8 #include <delay.h>
; 9 #include <math.h>
; 10
; 11 #define ROLE_4 PORTD.6
; 12 #define RELAY_3 PORTB.2
; 13 #define BANH_PHAI_D PORTD.1
; 14 #define BANH_TRAI_D PORTD.0
; 15 #define BANH_TRAI OCR1AL // PORTB.3
; 16 #define PWM_3 OCR0
; 17 #define BANH_PHAI OCR1BL
; 18 #define PWM4 OCR2 // PORT P.7
; 19 unsigned short PWM_VAL_L,PWM_VAL_R;// gia tri pwm dua vao hai banh
_PWM_VAL_L:
.BYTE 0x2
_PWM_VAL_R:
.BYTE 0x2
; 20 unsigned char PWM_COUNT=5;
_PWM_COUNT:
.BYTE 0x1
; 21
; 22 #define SPEAKER PORTC.7
; 23 #define START PINB.7// bit data in test button
; 24 #define CHON_DOI PINB.6// bit data in test button
; 25
; 26
; 27 ////////////////////////////////////////////////////////////////
; 28 #define SENSOR_L3 PINA.0//CAM BIEN TRAI NGOAI CUNG
; 29 #define SENSOR_L2 PINA.1
; 30 #define SENSOR_L1 PINA.2
; 31 #define SENSOR_LC PINA.3// CAM BIEN TRAI CHINH GIUA
; 32 #define SENSOR_RC PINA.4//CAM BIEN PHAI CHINH GIUA
; 33 #define SENSOR_R1 PINA.5
; 34 #define SENSOR_R2 PINA.6
; 35 #define SENSOR_R3 PINA.7// CAM BIEN PHAI NGOAI CUNG
; 36 //----------------------------------------------------------
; 37
; 38 //////
; 39 ///////////////////////////////// 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
; 40 unsigned char flash tdtraido[]={0,100, 90,100, 80, 100, 70, 120, 50, 150, 2, 170, 1, 175, 0, 200, 0};

.CSEG
; 41 unsigned char flash tdphaido[]={0,100, 100,90, 100, 80, 100, 70, 150, 50, 170, 5, 175, 2, 200, 0, 0};
; 42 ///////////////////////////////// 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
; 43 unsigned char flash tdtraitest[]={0,70, 60,70, 50, 70, 40, 70, 25, 80, 10, 100, 5, 120, 1, 150, 0};
; 44 unsigned char flash tdphaitest[]={0,70, 70,60, 70, 50, 70, 40, 80, 25, 100, 10, 120, 5, 150, 0, 0};
; 45 ////////////////////////////////////////////////////////////////////////////
; 46 ///////////////////////////////// 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
; 47 unsigned char flash tdtraithap[]={0,8, 7, 8, 6, 10, 5, 20, 4, 20, 3, 50, 1, 50, 0, 50, 0};
; 48 unsigned char flash tdphaithap[]={0,8, 8,7, 8, 6, 10, 5, 20, 4, 50, 3, 50, 1, 50, 0, 0};
; 49 ////////////////////////////////////////////////////////////////////////////
; 50 ///////////////////////////////// 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
; 51 unsigned char flash tdtraicao[]={0,200, 120, 200, 100, 200, 80, 200, 100, 200, 100, 200, 50, 230, 0, 250, 0};
; 52 unsigned char flash tdphaicao[]={0,200, 200, 275, 200, 100, 200, 80, 200, 130, 200, 80, 230, 50, 250, 0, 0};
; 53 bit BT1;
; 54 bit BT2;
; 55 bit BT3;
; 56 bit BT4;
; 57 bit BT5;
; 58 bit BT6;
; 59 bit BT7;
; 60 bit BT8;
; 61 bit BT9;
; 62 bit BT10;
; 63 bit BT11;
; 64 bit BT12;
; 65 bit BT13;
; 66 bit BT14;
; 67 bit BT15;
; 68 bit BT16;
; 69 bit CHE_DO;
; 70 bit TRAI;
; 71 bit PHAI;
; 72 bit LUI;
; 73 bit TIEN;
; 74 bit OK;
; 75 bit TANG;
; 76 bit GIAM;
; 77 bit BUSY_F;
; 78 bit STOP;
; 79 #define di_thang 0
; 80 #define phai 1
; 81 #define trai 2
; 82 unsigned char CAP_LECH,DEM_VACH,TONG_CB, ;

.DSEG
_CAP_LECH:
.BYTE 0x1
_DEM_VACH:
.BYTE 0x1
_TONG_CB:
.BYTE 0x1
; 83 bit ADJUST,BIT_QVN,BIT_NHO_PHAI,BIT_NHO_TRAI,CARRY_TRAI,CARRY_PHAI,BIT_CHONG_NHIEU,C;
; 84 ///////////////////////////////////////////////////////////////////////
; 85
; 86 ///////////////////////////////////// khia bao cac ham dang marcro
; 87 void phan_loai(void);
; 88 void xu_ly_td_test(void);
; 89 void kt_qvn(void);
; 90 void re_trai_90(void);
; 91 void test_re_trai_90(void);
; 92 void turn_left_90(void);
; 93 void re_phai_90(void);
; 94 void test_re_phai_90(void);
; 95 void turn_right_90(void);
; 96 void tien_do_duong_td_test(void);
; 97 void dung(void);
; 98 void breaker(void);
; 99
; 100
; 101 void chay_mu(void);
; 102 void kiem_tra_ht(void);
; 103 void xu_ly_do_cao(void);
; 104 void khoi_hanh(void);
; 105
; 106 ////-----------------------------------------------------------------------------------------
; 107 void chong_nhieu(void)
; 108 {

.CSEG
; 109 BANH_TRAI_D=1;
; 110 BANH_PHAI_D=1;
; 111 BANH_TRAI=60;
; 112 BANH_PHAI=60;
; 113 delay_ms(2);
; 114 BANH_TRAI=0;
; 115 BANH_PHAI=0;
; 116 }
; 117
; 118 void sound(void)
; 119 {
; 120 unsigned char notf=0;
; 121
; 122 for (notf=0;notf<=150;notf++)
; notf -> R16
; 123 {
; 124 SPEAKER=!SPEAKER;
; 125 delay_us(40);
; 126 SPEAKER=1;
; 127 }
; 128
; 129 SPEAKER=1;
; 130 }
; 131
; 132 void phan_loai(void)// VACH TRANG UNG VOI CHAN INPUT O MUC 1 , VACH XANH UNG VOI DAU VAO INPUT O MUC 0
; 133 {
; 134 BIT_NHO_TRAI=0;
; 135 BIT_NHO_PHAI=0;
; 136 CAP_LECH=0;
; 137 C=0;
; 138 while(1)
; 139 {
; 140 BIT_NHO_TRAI=0;
; 141 BIT_NHO_PHAI=0;
; 142 C=1;
; 143 C=SENSOR_LC&SENSOR_RC&!SENSOR_L1&!SENSOR_R1;// hai cam bien trung tam nam trong vach
; 144 if (C==0) ////0 0 0 1 1 0 0 0
; 145 {
; 146 goto LECH_PHAI1;
; 147 }
; 148 else if (C==1)
; 149 {
; 150 CAP_LECH=1;
; 151 C=0;
; 152 break;
; 153 }
; 154 LECH_PHAI1:
; 155 C=1;
; 156
; 157 C= !SENSOR_L1&!SENSOR_RC&SENSOR_LC&!SENSOR_R1&!SENSOR_R2&!SENSOR_R3&!SENSOR_L2&!SENSOR_L3;// hoi lech sang trai
; 158 if (C==0) ////0 0 0 1 0 0 0 0
; 159 {
; 160 goto LECH_TRAI1;
; 161 }
; 162 else if (C==1)
; 163 {
; 164 CAP_LECH=2;
; 165 C=0;
; 166 break;
; 167 }
; 168 LECH_TRAI1:
; 169
; 170 C=1;
; 171 C= !SENSOR_LC&!SENSOR_R1&SENSOR_RC;//hoi lech sang phai
; 172 if (C==0) ////0 0 0 0 1 0 0 0
; 173 {
; 174 goto LECH_PHAI2;
; 175 }
; 176 else if (C==1)
; 177 {
; 178 CAP_LECH=3;
; 179 C=0;
; 180 break;
; 181 }
; 182 LECH_PHAI2:
; 183 C=1;
; 184
; 185 C= SENSOR_L1&SENSOR_LC;
; 186 if (C==0) ////0 0 1 1 0 0 0 0
; 187 {
; 188 goto LECH_TRAI2;
; 189 }
; 190 else if (C==1)
; 191 {
; 192 CAP_LECH=4;
; 193 C=0;
; 194 break;
; 195 }
; 196 LECH_TRAI2:
; 197 C=1;
; 198
; 199 C= SENSOR_R1&SENSOR_RC;
; 200 if (C==0) ////0 0 0 0 1 1 0 0
; 201 {
; 202 goto LECH_PHAI3;
; 203 }
; 204 else if (C==1)
; 205 {
; 206 CAP_LECH=5;
; 207 C=0;
; 208 break;
; 209 }
; 210 LECH_PHAI3:
; 211 C=1;
; 212
; 213 C= !SENSOR_L2&!SENSOR_LC&SENSOR_L1; ////0 0 1 0 0 0 0 0
; 214 if (C==0)
; 215 {
; 216 goto LECH_TRAI3;
; 217 }
; 218 else if (C==1)
; 219 {
; 220 CAP_LECH=6;
; 221 C=0;
; 222 break;
; 223 }
; 224 LECH_TRAI3:
; 225 C=1;
; 226
; 227 C= !SENSOR_R2&!SENSOR_RC&SENSOR_R1;/////0 0 0 0 0 1 0 0
; 228 if (C==0)
; 229 {
; 230 goto LECH_PHAI4;
; 231 }
; 232 else if (C==1)
; 233 {
; 234 CAP_LECH=7;
; 235 C=0;
; 236 break;
; 237 }
; 238 LECH_PHAI4:
; 239 C=1;
; 240
; 241 C= SENSOR_L2&SENSOR_L1; ////0 1 1 0 0 0 0 0
; 242 if (C==0)
; 243 {
; 244 goto LECH_TRAI4;
; 245 }
; 246 else if (C==1)
; 247 {
; 248 CAP_LECH=8;
; 249
; 250 C=0;
; 251 break;
; 252 }
; 253 LECH_TRAI4:
; 254 C=1;
; 255
; 256 C= SENSOR_R2&SENSOR_R1; /////0 0 0 0 0 1 1 0
; 257 if (C==0)
; 258 {
; 259 goto LECH_PHAI5;
; 260 }
; 261 else if (C==1)
; 262 {
; 263 CAP_LECH=9;
; 264 C=0;
; 265 break;
; 266 }
; 267 LECH_PHAI5:
; 268 C=1;
; 269
; 270 C= (SENSOR_L2&!SENSOR_L1&!SENSOR_L3&!SENSOR_LC); ////0 1 0 0 0 0 0 0
; 271 if (C==0)
; 272 {
; 273 goto LECH_TRAI5;
; 274 }
; 275 else if (C==1)
; 276 {
; 277 CAP_LECH=10;
; 278 C=0;
; 279 break;
; 280 }
; 281
; 282 LECH_TRAI5:
; 283 C=1;
; 284
; 285 C= !SENSOR_R3&!SENSOR_R1&SENSOR_R2; ////0 0 0 0 0 0 1 0
; 286 if (C==0)
; 287 {
; 288 goto LECH_PHAI6;
; 289 }
; 290 else if (C==1)
; 291 {
; 292 CAP_LECH=11;
; 293 C=0;
; 294 break;
; 295 }
; 296 LECH_PHAI6:
; 297 C=1;
; 298
; 299 C= SENSOR_L3&SENSOR_L2; ////1 1 0 0 0 0 0 0
; 300 if (C==0)
; 301 {
; 302 goto LECH_TRAI6;
; 303 }
; 304 else if (C==1)
; 305 {
; 306 CAP_LECH=12;
; 307 C=0;
; 308 break;
; 309 }
; 310 LECH_TRAI6:
; 311 C=1;
; 312
; 313 C= (SENSOR_R3&SENSOR_R2&!SENSOR_R1); ////0 0 0 0 0 0 1 1
; 314 if (C==0)
; 315 {
; 316 goto LECH_PHAI7;
; 317 }
; 318 else if (C==1)
; 319 {
; 320 CAP_LECH=13;
; 321 C=0;
; 322 break;
; 323 }
; 324 LECH_PHAI7:
; 325 C=1;
; 326
; 327 C= !SENSOR_L2&SENSOR_L3; ////1 0 0 0 0 0 0 0
; 328 if (C==0)
; 329 {
; 330 goto LECH_TRAI7;
; 331 }
; 332 else if (C==1)
; 333 {
; 334 CAP_LECH=14;
; 335 CARRY_PHAI=1;
; 336 C=0;
; 337 break;
; 338 }
; 339 LECH_TRAI7:
; 340 C=1;
; 341
; 342 C=(SENSOR_R3&!SENSOR_R2&!SENSOR_R1); ////0 0 0 0 0 0 0 1
; 343 if (C==0)
; 344 {
; 345 goto ERROR_PHAI;
; 346 }
; 347 else if (C==1)
; 348 {
; 349 CAP_LECH=15;
; 350 CARRY_TRAI=1;
; 351 C=0;
; 352 break;
; 353 }
; 354 ERROR_PHAI:
; 355 C=1;
; 356
; 357 C=CARRY_PHAI&(!SENSOR_L3&!SENSOR_L2&!SENSOR_L1&!SENSOR_LC&!SENSOR_RC&!SENSOR_R1&!SENSOR_R2&!SENSOR_R3);
; 358 // 1 0 0 0 0 0 0 0 0
; 359 if (C==0)
; 360 {
; 361 goto ERROR_TRAI;
; 362 }
; 363 else if (C==1)
; 364 {
; 365 BIT_NHO_PHAI=1;
; 366 CARRY_PHAI=0;
; 367 C=0;
; 368 break;
; 369 }
; 370 ERROR_TRAI:
; 371 C=1;
; 372
; 373 C=CARRY_TRAI&(!SENSOR_L3&!SENSOR_L2&!SENSOR_L1&!SENSOR_LC&!SENSOR_RC&!SENSOR_R1&!SENSOR_R2&!SENSOR_R3);
; 374 /// 0 0 0 0 0 0 0 0 1
; 375 if (C==0)
; 376 {
; 377 goto NO_LINE;
; 378 }
; 379 else if (C==1)
; 380 {
; 381 BIT_NHO_TRAI=1;
; 382 CARRY_TRAI=0;
; 383 C=0;
; 384 break;
; 385 }
; 386 NO_LINE:
; 387 C=1;
; 388
; 389 C=!SENSOR_L3&!SENSOR_L2&!SENSOR_L1&!SENSOR_LC&!SENSOR_RC&!SENSOR_R1&!SENSOR_R2&!SENSOR_R3;
; 390 /// 0 0 0 0 0 0 0 0
; 391 if (C==0)
; 392 {
; 393 goto EXIT_TEST;
; 394 }
; 395 else if (C==1)
; 396 {
; 397 CAP_LECH=16;// ROBOT DUNG YEN
; 398 C=0;
; 399 break;
; 400 }
; 401 EXIT_TEST:
; 402
; 403 dung();
; 404 break; // khong phai vung lech nao thi cung thoat
; 405
; 406 }// ket thuc vong lap while
; 407
; 408 }///////////////////////////////////////
; 409 /////////////////////////////////////////////////////////////////////////
; 410 void xu_ly_td_test(void)// ok
; 411 {
; 412 BANH_TRAI_D=1;
; 413 BANH_PHAI_D=1;
; 414 BANH_TRAI=tdtraitest[CAP_LECH];
; 415 BANH_PHAI=tdphaitest[CAP_LECH];
; 416 if (BIT_NHO_TRAI==1)
; 417 {
; 418 BIT_NHO_TRAI=0;// khoi tao lai bit nho trai
; 419 BANH_TRAI_D=0;
; 420 BANH_PHAI_D=0;
; 421 delay_ms(10);
; 422 do
; 423 {
; 424 BANH_PHAI=70;
; 425 BANH_TRAI=5;
; 426 }
; 427 while(!SENSOR_RC);
; 428 }
; 429
; 430 if (BIT_NHO_PHAI==1)
; 431 {
; 432 BIT_NHO_PHAI=0;// khoi tao lai bit nho phai
; 433 BANH_TRAI_D=0;
; 434 BANH_PHAI_D=0;
; 435 delay_ms(10);
; 436 do
; 437 {
; 438 BANH_PHAI=5;
; 439 BANH_TRAI=70;
; 440 }
; 441 while(!SENSOR_LC);
; 442 }
; 443 }
; 444
; 445 //////////////////////////
; 446 void xu_ly_td_thap(void)// ok
; 447 {
; 448 BANH_TRAI_D=1;
; 449 BANH_PHAI_D=1;
; 450 BANH_TRAI=tdtraithap[CAP_LECH];
; 451 BANH_PHAI=tdphaithap[CAP_LECH];
; 452 if (BIT_NHO_TRAI==1)
; 453 {
; 454 BIT_NHO_TRAI=0;// khoi tao lai bit nho trai
; 455 BANH_TRAI_D=0;
; 456 BANH_PHAI_D=0;
; 457 delay_ms(10);
; 458 do
; 459 {
; 460 BANH_PHAI=60;
; 461 BANH_TRAI=5;
; 462 }
; 463 while(!SENSOR_R2);
; 464 }
; 465
; 466 if (BIT_NHO_PHAI==1)
; 467 {
; 468 BIT_NHO_PHAI=0;// khoi tao lai bit nho phai
; 469 BANH_TRAI_D=0;
; 470 BANH_PHAI_D=0;
; 471 delay_ms(10);
; 472 do
; 473 {
; 474 BANH_PHAI=5;
; 475 BANH_TRAI=60;
; 476 }
; 477 while(!SENSOR_L2);
; 478 }
; 479 }
; 480 /////////////////////////////
; 481 void xu_ly_td_cao(void)// ok
; 482 {
; 483 BANH_TRAI_D=1;
; 484 BANH_PHAI_D=1;
; 485 BANH_TRAI=tdtraicao[CAP_LECH];
; 486 BANH_PHAI=tdphaicao[CAP_LECH];
; 487 if (BIT_NHO_TRAI==1)
; 488 {
; 489 BIT_NHO_TRAI=0;// khoi tao lai bit nho trai
; 490 BANH_TRAI_D=0;
; 491 BANH_PHAI_D=0;
; 492 delay_ms(10);
; 493 do
; 494 {
; 495 BANH_PHAI=150;
; 496 BANH_TRAI=0;
; 497 }
; 498 while(!SENSOR_R2);
; 499 }
; 500
; 501 if (BIT_NHO_PHAI==1)
; 502 {
; 503 BIT_NHO_PHAI=0;// khoi tao lai bit nho phai
; 504 BANH_TRAI_D=0;
; 505 BANH_PHAI_D=0;
; 506 delay_ms(10);
; 507 do
; 508 {
; 509 BANH_PHAI=1;
; 510 BANH_TRAI=150;
; 511 }
; 512 while(!SENSOR_L2);
; 513 }
; 514 }
; 515 //-------------------------------------------
; 516 void kt_qvn(void)//ok
; 517 {
; 518 BIT_QVN=0;// khoi dong bit kiem tra qua vach ngang
; 519 TONG_CB=0;
; 520 if (SENSOR_L3==1) TONG_CB++;
; 521 if (SENSOR_L2==1) TONG_CB++;
; 522 if (SENSOR_L1==1) TONG_CB++;
; 523 if (SENSOR_LC==1) TONG_CB++;
; 524 if (SENSOR_RC==1) TONG_CB++;
; 525 if (SENSOR_R1==1) TONG_CB++;
; 526 if (SENSOR_R2==1) TONG_CB++;
; 527 if (SENSOR_R3==1) TONG_CB++;
; 528 if (TONG_CB>=4)// neu tong cam bien 4
; 529 {
; 530 do // kiem tra tiep toan bo tong cam bien
; 531 {
; 532 TONG_CB=0;// RESET TONG CAM BIEN
; 533 if (SENSOR_L3==1) TONG_CB++;
; 534 if (SENSOR_L2==1) TONG_CB++;
; 535 if (SENSOR_L1==1) TONG_CB++;
; 536 if (SENSOR_LC==1) TONG_CB++;
; 537 if (SENSOR_RC==1) TONG_CB++;
; 538 if (SENSOR_R1==1) TONG_CB++;
; 539 if (SENSOR_R2==1) TONG_CB++;
; 540 if (SENSOR_R3==1) TONG_CB++;
; 541 }
; 542 while(TONG_CB>=3);// cho den khi tong cam bien nho hon 3 thi xac nhan da qua vach ngang
; 543 BIT_QVN=1;// thiet lap bit qua vach ngang
; 544 //sound();
; 545 //SPEAKER=1;
; 546 }
; 547 }/////////
; 548 void do_duong_td_test(void)
; 549 {
; 550 BIT_QVN=0;// KHOI TAO BIT XAC NHAN QUA VACH NGANG
; 551 do
; 552 {
; 553 phan_loai();
; 554 xu_ly_td_test();
; 555 kt_qvn();
; 556 }
; 557 while(BIT_QVN==0);
; 558 }
; 559 ///////////////////////////////////////////
; 560 void tien_do_duong_td_test()
; 561 {
; 562 phan_loai();
; 563 xu_ly_td_test();
; 564 }
; 565 //////////////////////
; 566 void tien_do_duong_td_cao()
; 567 {
; 568 phan_loai();
; 569 xu_ly_td_cao();
; 570 }
; 571 //////////////////////
; 572 void do_duong_td_thap(void)
; 573 {
; 574 BIT_QVN=0;// KHOI TAO BIT XAC NHAN QUA VACH NGANG
; 575 do
; 576 {
; 577 phan_loai();
; 578 xu_ly_td_thap();
; 579 kt_qvn();
; 580 }
; 581 while(BIT_QVN==0);
; 582 }
; 583 ///////////////////////////////////////
; 584 void do_duong_td_cao(void)
; 585 {
; 586 BIT_QVN=0;// KHOI TAO BIT XAC NHAN QUA VACH NGANG
; 587 do
; 588 {
; 589 phan_loai();
; 590 xu_ly_td_cao();
; 591 kt_qvn();
; 592 }
; 593 while(BIT_QVN==0);
; 594 }
; 595
; 596 //xu li khi robot dung o nga tu
; 597 void nga_tu_tdthap(unsigned char turn)
; 598 {
; 599 bit re=1;
; 600 bit tre_re=0;
; 601 while (re==1)
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 602 {
; 603 if (turn==phai)
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 604 {
; 605 BANH_TRAI_D=1;
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 606 BANH_PHAI_D=0;
; 607 delay_ms(20);
; 608 BANH_TRAI=70;
; 609 BANH_PHAI=2;
; 610 }
; 611 if (turn==trai)
; 612 {
; 613 BANH_TRAI_D=0;
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 614 BANH_PHAI_D=1;
; 615 delay_ms(20);
; 616 BANH_TRAI=2;
; 617 BANH_PHAI=70;
; 618 }
; 619 if (tre_re==0)
; 620 {
; 621 tre_re=1;
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 622 if (turn==phai)
; 623 {
; 624 delay_ms(30);
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 625 }
; 626 if (turn==trai)
; 627 {
; 628 delay_ms(30);
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 629 }
; 630 }
; 631 }
; 632 }
; 633 void nga_tu_tdtrungbinh(unsigned char turn)
; 634 {
; 635 bit re=1;
; 636 bit tre_re=0;
; 637 while (re==1)
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 638 {
; 639 if (turn==phai)
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 640 {
; 641 BANH_TRAI_D=1;
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 642 BANH_PHAI_D=0;
; 643 delay_ms(20);
; 644 BANH_TRAI=150;
; 645 BANH_PHAI=30;
; 646 }
; 647 if (turn==trai)
; 648 {
; 649 BANH_TRAI_D=0;
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 650 BANH_PHAI_D=1;
; 651 delay_ms(20);
; 652 BANH_TRAI=30;
; 653 BANH_PHAI=150;
; 654 }
; 655 if (tre_re==0)
; 656 {
; 657 tre_re=1;
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 658 if (turn==phai)
; 659 {
; 660 delay_ms(15);
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 661 }
; 662 if (turn==trai)
; 663 {
; 664 delay_ms(15);
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
; 665 }
; 666 }
; 667 }
; 668 }
; 669 void nga_tu_tdcao(unsigned char turn)
; 670 {
_nga_tu_tdcao:
PUSH R15
; 671 bit re=1;
; 672 bit tre_re=0;
; 673 while (re==1)
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
MOV R30,R15
ANDI R30,LOW(0xFC)
ORI R30,LOW(0x1)
MOV R15,R30
_0x97:
SBRS R15,0
RJMP _0x99
; 674 {
; 675 if (turn==phai)
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
LD R26,Y
CPI R26,LOW(0x1)
BRNE _0x9A
; 676 {
; 677 BANH_TRAI_D=1;
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
SBI 0x12,0
; 678 BANH_PHAI_D=0;
CBI 0x12,1
; 679 delay_ms(20);
CALL SUBOPT_0x0
; 680 BANH_TRAI=200;
LDI R30,LOW(200)
OUT 0x2A,R30
; 681 BANH_PHAI=50;
LDI R30,LOW(50)
OUT 0x28,R30
; 682 delay_ms(50);
LDI R30,LOW(50)
LDI R31,HIGH(50)
CALL SUBOPT_0x1
; 683 }
; 684 if (turn==trai)
_0x9A:
LD R26,Y
CPI R26,LOW(0x2)
BRNE _0x9B
; 685 {
; 686 BANH_TRAI_D=0;
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
CBI 0x12,0
; 687 BANH_PHAI_D=1;
SBI 0x12,1
; 688 delay_ms(20);
CALL SUBOPT_0x0
; 689 BANH_TRAI=50;
LDI R30,LOW(50)
OUT 0x2A,R30
; 690 BANH_PHAI=200;
LDI R30,LOW(200)
OUT 0x28,R30
; 691 }
; 692 if (tre_re==0)
_0x9B:
SBRC R15,1
RJMP _0x9C
; 693 {
; 694 tre_re=1;
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
SET
BLD R15,1
; 695 if (turn==phai)
LD R26,Y
CPI R26,LOW(0x1)
BRNE _0x9D
; 696 {
; 697 delay_ms(60);
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
LDI R30,LOW(60)
LDI R31,HIGH(60)
CALL SUBOPT_0x1
; 698 }
; 699 if (turn==trai)
_0x9D:
LD R26,Y
CPI R26,LOW(0x2)
BRNE _0x9E
; 700 {
; 701 delay_ms(60);
; turn -> Y+0
; re -> R15.0
; tre_re -> R15.1
LDI R30,LOW(60)
LDI R31,HIGH(60)
CALL SUBOPT_0x1
; 702 }
; 703 }
_0x9E:
; 704 }
_0x9C:
RJMP _0x97
_0x99:
; 705 }
ADIW R28,1
POP R15
RET
; 706
; 707 void dung(void)
; 708 {
; 709 BANH_TRAI=0;
; 710 BANH_PHAI=0;
; 711 BANH_TRAI_D=1;
; 712 BANH_PHAI_D=1;
; 713 }
; 714 //--------------------------
; 715 void breaker(void)
; 716 {
; 717 BANH_TRAI=0;
; 718 BANH_PHAI=0;
; 719 BANH_TRAI_D=0;
; 720 BANH_PHAI_D=0;
; 721 }
; 722 //--------------------------
; 723 void tien_td_test(void)
; 724 {
; 725 BANH_TRAI_D=1;
; 726 BANH_PHAI_D=1;
; 727 do_duong_td_test();
; 728 }
; 729 //---------------------------------------------
; 730
; 731 void lui_1_doan(void)
; 732 {
; 733 int n,i;
; 734 n=7;
; n -> R16,R17
; i -> R18,R19
; 735 for(i=1;i<=n;i++)
; 736 {
; 737 BANH_TRAI_D=0;
; 738 BANH_PHAI_D=0;
; 739 delay_ms(2);
; 740 BANH_TRAI=50;
; 741 BANH_PHAI=50;
; 742 delay_ms(1);
; 743 }
; 744
; 745 dung();
; 746 }
; 747 ///////////------chay_mu---------------
; 748 void khoi_hanh_2(void)
; 749 {
; 750 BANH_TRAI_D=1;
; 751 BANH_PHAI_D=1;
; 752 BANH_TRAI=20;
; 753 BANH_PHAI=20;
; 754 delay_ms(10);
; 755 }
; 756 void khoi_hanh_1(void)
; 757 {
; 758 BANH_TRAI_D=1;
; 759 BANH_PHAI_D=1;
; 760 BANH_TRAI=10;
; 761 BANH_PHAI=10;
; 762 delay_ms(10);
; 763
; 764 BANH_TRAI_D=1;
; 765 BANH_PHAI_D=1;
; 766 BANH_TRAI=80;
; 767 BANH_PHAI=80;
; 768 delay_ms(5);
; 769
; 770 BANH_TRAI_D=1;
; 771 BANH_PHAI_D=1;
; 772 BANH_TRAI=80;
; 773 BANH_PHAI=80;
; 774 delay_ms(5);
; 775
; 776 BANH_TRAI_D=1;
; 777 BANH_PHAI_D=1;
; 778 BANH_TRAI=100;
; 779 BANH_PHAI=100;
; 780 delay_ms(5);
; 781 }
; 782 void khoi_hanh(void)
; 783 {
; 784 BANH_TRAI_D=1;
; 785 BANH_PHAI_D=1;
; 786 BANH_TRAI=5;
; 787 BANH_PHAI=5;
; 788 delay_ms(10);
; 789
; 790 BANH_TRAI_D=1;
; 791 BANH_PHAI_D=1;
; 792 BANH_TRAI=10;
; 793 BANH_PHAI=10;
; 794 delay_ms(10);
; 795
; 796 BANH_TRAI_D=1;
; 797 BANH_PHAI_D=1;
; 798 BANH_TRAI=15;
; 799 BANH_PHAI=15;
; 800 delay_ms(10);
; 801
; 802 BANH_TRAI_D=1;
; 803 BANH_PHAI_D=1;
; 804 BANH_TRAI=20;
; 805 BANH_PHAI=20;
; 806 delay_ms(5);
; 807
; 808 BANH_TRAI_D=1;
; 809 BANH_PHAI_D=1;
; 810 BANH_TRAI=30;
; 811 BANH_PHAI=30;
; 812 delay_ms(5);
; 813
; 814 BANH_TRAI_D=1;
; 815 BANH_PHAI_D=1;
; 816 BANH_TRAI=35;
; 817 BANH_PHAI=35;
; 818 delay_ms(5);
; 819 }
; 820 //////////////////////////////////
; 821 void chay_thang(void)
; 822 {
; 823
; 824 BANH_TRAI_D=1;
; 825 BANH_PHAI_D=1;
; 826 BANH_TRAI=30;
; 827 BANH_PHAI=30;
; 828 delay_ms(10);
; 829
; 830 BANH_TRAI_D=1;
; 831 BANH_PHAI_D=1;
; 832 BANH_TRAI=20;
; 833 BANH_PHAI=20;
; 834 delay_ms(5);
; 835
; 836 BANH_TRAI_D=1;
; 837 BANH_PHAI_D=1;
; 838 BANH_TRAI=15;
; 839 BANH_PHAI=15;
; 840 delay_ms(5);
; 841
; 842 BANH_TRAI_D=1;
; 843 BANH_PHAI_D=1;
; 844 BANH_TRAI=10;
; 845 BANH_PHAI=10;
; 846 delay_ms(5);
; 847
; 848 BANH_TRAI_D=1;
; 849 BANH_PHAI_D=1;
; 850 BANH_TRAI=5;
; 851 BANH_PHAI=5;
; 852 delay_ms(2);
; 853
; 854 BANH_TRAI_D=1;
; 855 BANH_PHAI_D=1;
; 856 BANH_TRAI=2;
; 857 BANH_PHAI=2;
; 858 delay_ms(2);
; 859
; 860
; 861 dung();
; 862 }
; 863 //------------------------ -------------------------
; 864
; 865 void chay_thang_1(void)
; 866 {
; 867 BANH_TRAI_D=1;
; 868 BANH_PHAI_D=1;
; 869 BANH_TRAI=30;
; 870 BANH_PHAI=30;
; 871 delay_ms(10);
; 872
; 873 BANH_TRAI_D=1;
; 874 BANH_PHAI_D=1;
; 875 BANH_TRAI=20;
; 876 BANH_PHAI=20;
; 877 delay_ms(10);
; 878
; 879 BANH_TRAI_D=1;
; 880 BANH_PHAI_D=1;
; 881 BANH_TRAI=15;
; 882 BANH_PHAI=15;
; 883 delay_ms(5);
; 884
; 885 BANH_TRAI_D=1;
; 886 BANH_PHAI_D=1;
; 887 BANH_TRAI=10;
; 888 BANH_PHAI=10;
; 889 delay_ms(5);
; 890
; 891 BANH_TRAI_D=1;
; 892 BANH_PHAI_D=1;
; 893 BANH_TRAI=5;
; 894 BANH_PHAI=5;
; 895 delay_ms(10);
; 896
; 897 BANH_TRAI_D=1;
; 898 BANH_PHAI_D=1;
; 899 BANH_TRAI=2;
; 900 BANH_PHAI=2;
; 901 delay_ms(10);
; 902
; 903 dung();
; 904 }
; 905 //-------------------------------
; 906 void chay_thang_2(void)
; 907 {
; 908
; 909 tien_do_duong_td_test();
; 910 tien_do_duong_td_test();
; 911 tien_do_duong_td_test();
; 912 tien_do_duong_td_test();
; 913 tien_do_duong_td_test();
; 914 tien_do_duong_td_test();
; 915
; 916 BANH_TRAI_D=1;
; 917 BANH_PHAI_D=1;
; 918 BANH_TRAI=30;
; 919 BANH_PHAI=30;
; 920 delay_ms(20);
; 921
; 922 BANH_TRAI_D=1;
; 923 BANH_PHAI_D=1;
; 924 BANH_TRAI=20;
; 925 BANH_PHAI=20;
; 926 delay_ms(20);
; 927
; 928 BANH_TRAI_D=1;
; 929 BANH_PHAI_D=1;
; 930 BANH_TRAI=15;
; 931 BANH_PHAI=15;
; 932 delay_ms(15);
; 933
; 934 BANH_TRAI_D=1;
; 935 BANH_PHAI_D=1;
; 936 BANH_TRAI=10;
; 937 BANH_PHAI=10;
; 938 delay_ms(10);
; 939
; 940 BANH_TRAI_D=1;
; 941 BANH_PHAI_D=1;
; 942 BANH_TRAI=5;
; 943 BANH_PHAI=5;
; 944 delay_ms(10);
; 945
; 946 BANH_TRAI_D=1;
; 947 BANH_PHAI_D=1;
; 948 BANH_TRAI=2;
; 949 BANH_PHAI=2;
; 950 delay_ms(10);
; 951
; 952
; 953 dung();
; 954 }
; 955 void thiet_lap_trang_thai_ban_dau_co_cau_nang(void)
; 956 {
; 957
; 958 }
; 959 void chay_3_vach(void) //chay 3vach roi dung roi dung dua khoi vao
; 960 {
; 961 if (DEM_VACH==3)
; 962 {
; 963 dung();
; 964 //chuong trinh dua khoi vao
; 965 delay_ms(30);
; 966 }
; 967 }
; 968 void Mankauraa(void)
; 969 {
; 970 khoi_hanh_2();
; 971 for (DEM_VACH=1;DEM_VACH<=3;DEM_VACH++) // chay thanh 3 vach
; 972 {
; 973 do_duong_td_test();
; 974 }
; 975
; 976 chay_3_vach();
; 977 }
; 978
; 979
; 980
; 981 void init_PORT(void)
; 982 {
_init_PORT:
; 983 //PORTA State7=P State6=P State5=P State4=P State3=P State2=P State1=P State0=P
; 984 PORTA=0xFF;
LDI R30,LOW(255)
OUT 0x1B,R30
; 985 DDRA=0x00;
LDI R30,LOW(0)
OUT 0x1A,R30
; 986
; 987 //PORTB State7=P State6=P State5=P State4=P State3=0 State2=0 State1=0 State0=0
; 988 PORTB=0xF0;
LDI R30,LOW(240)
OUT 0x18,R30
; 989 DDRB=0x0F;
LDI R30,LOW(15)
OUT 0x17,R30
; 990
; 991 //PORTC State7=0 State6=P State5=P State4=0 State3=0 State2=0 State1=P State0=P
; 992 PORTC=0x63;
LDI R30,LOW(99)
OUT 0x15,R30
; 993 DDRC=0x9C;
LDI R30,LOW(156)
OUT 0x14,R30
; 994
; 995 //PORTD State7=0 State6=0 State5=0 State4=0 State3=P State2=P State1=0 State0=0
; 996 PORTD=0x0C;
LDI R30,LOW(12)
OUT 0x12,R30
; 997 DDRD=0xF3;
LDI R30,LOW(243)
OUT 0x11,R30
; 998 }
RET
; 999 void init_timer(void)
; 1000 {
_init_timer:
; 1001 //timer 1,Phase correct PWM top=FFh,OC0 output: Inverted PWM,Clock value: 250.000 kHz
; 1002 TCCR0=0x73;
LDI R30,LOW(115)
OUT 0x33,R30
; 1003 TCNT0=0x00;
LDI R30,LOW(0)
OUT 0x32,R30
; 1004 OCR0=0x00;
OUT 0x3C,R30
; 1005
; 1006 //timer 1,Ph. correct PWM top=00FFh,Inverted,Clock value: 250.000 kHz
; 1007 TCCR1A=0xF1;
LDI R30,LOW(241)
OUT 0x2F,R30
; 1008 TCCR1B=0x03;
LDI R30,LOW(3)
OUT 0x2E,R30
; 1009 TCNT1H=0x00;
LDI R30,LOW(0)
OUT 0x2D,R30
; 1010 TCNT1L=0x00;
OUT 0x2C,R30
; 1011 ICR1H=0x00;
OUT 0x27,R30
; 1012 ICR1L=0x00;
OUT 0x26,R30
; 1013 OCR1AH=0x00;
OUT 0x2B,R30
; 1014 OCR1AL=0x00;
OUT 0x2A,R30
; 1015 OCR1BH=0x00;
OUT 0x29,R30
; 1016 OCR1BL=0x00;
OUT 0x28,R30
; 1017
; 1018 //timer 2,Phase correct PWM top=FFh,OC2 output: Inverted PWM, Clock value: 250.000 kHz,
; 1019 ASSR=0x00;
OUT 0x22,R30
; 1020 TCCR2=0x74;
LDI R30,LOW(116)
OUT 0x25,R30
; 1021 TCNT2=0x00;
LDI R30,LOW(0)
OUT 0x24,R30
; 1022 OCR2=0x00;
OUT 0x23,R30
; 1023 }
RET
; 1024 void main(void)
; 1025 {
_main:
; 1026 // Declare your local variables here
; 1027 SREG.7=0;
BCLR 7
; 1028 init_PORT();
CALL _init_PORT
; 1029 init_timer();
CALL _init_timer
; 1030 SREG.7=1;
BSET 7
; 1031
; 1032
; 1033
; 1034
; 1035 ////////////////////////////////////////////////doan chuong trinh nam ngoai vong lap While
; 1036 //reset_keypad();
; 1037 SPEAKER=1;
SBI 0x15,7
; 1038 delay_us(20);//chong doi
__DELAY_USB 107
; 1039 PWM_3=0;
LDI R30,LOW(0)
OUT 0x3C,R30
; 1040 BANH_TRAI=0;
OUT 0x2A,R30
; 1041 BANH_PHAI=0;
OUT 0x28,R30
; 1042
; 1043 RELAY_3=1;
SBI 0x18,2
; 1044 BANH_TRAI_D=1;
SBI 0x12,0
; 1045 BANH_PHAI_D=1;
SBI 0x12,1
; 1046
; 1047 //assign_variable();// gan gia tri sau khi kiem tra phim bam
; 1048 delay_ms(100);// doi he thong hoat dong on dinh
LDI R30,LOW(100)
LDI R31,HIGH(100)
CALL SUBOPT_0x1
; 1049 /////////////////////////////////////chay thu san////////////////////////////////////
; 1050
; 1051 //chay_thang_2();
; 1052 BANH_TRAI_D=0;
CBI 0x12,0
; 1053 BANH_PHAI_D=1;
SBI 0x12,1
; 1054 delay_ms(30);
LDI R30,LOW(30)
LDI R31,HIGH(30)
CALL SUBOPT_0x1
; 1055 // Mankauraa() ;
; 1056
; 1057 BANH_TRAI_D=0;
CBI 0x12,0
; 1058 BANH_PHAI_D=1;
SBI 0x12,1
; 1059 nga_tu_tdcao(phai);
LDI R30,LOW(1)
ST -Y,R30
CALL _nga_tu_tdcao
; 1060 //delay_ms(100);
; 1061 //dung();
; 1062 //start();
; 1063 //chon_doi();
; 1064
; 1065
; 1066 //---------------------------------------------------------------
; 1067 }
_0xA6:
RJMP _0xA6


;OPTIMIZER ADDED SUBROUTINE, CALLED 2 TIMES, CODE SIZE REDUCTION:1 WORDS
SUBOPT_0x0:
LDI R30,LOW(20)
LDI R31,HIGH(20)
ST -Y,R31
ST -Y,R30
JMP _delay_ms

;OPTIMIZER ADDED SUBROUTINE, CALLED 5 TIMES, CODE SIZE REDUCTION:5 WORDS
SUBOPT_0x1:
ST -Y,R31
ST -Y,R30
JMP _delay_ms

_delay_ms:
ld r30,y+
ld r31,y+
adiw r30,0
breq __delay_ms1
__delay_ms0:
__DELAY_USW 0xFA0
wdr
sbiw r30,1
brne __delay_ms0
__delay_ms1:
ret

__SAVELOCR4:
ST -Y,R19
__SAVELOCR3:
ST -Y,R18
__SAVELOCR2:
ST -Y,R17
ST -Y,R16
RET

__LOADLOCR4:
LDD R19,Y+3
__LOADLOCR3:
LDD R18,Y+2
__LOADLOCR2:
LDD R17,Y+1
LD R16,Y
RET

;END OF CODE MARKER
__END_OF_CODE:


Đăng By ChipKool