.include "2313def.inc"

; секция двигателей
.EQU 	go_f= 0b00011001	; оба вперед
.EQU	go_r= 0b00010001	; правый вперед
.EQU	go_l= 0b00011000	; левый вперед
.EQU	go_b= 0b00010110	; оба назад
.EQU	go_lr=0b00010101	; левый правый
.EQU	go_rl=0b00011010	; правый левый
.EQU 	MOTORDDR=DDRB
.EQU 	MOTORPORT=PORTB
.EQU 	wait_motor=2
.EQU 	motor_bcount=30

.DEF 	reg_motor=R24
.DEF 	reg_count=R25


; секция сенсора края
.EQU	botsensor1=2
.EQU	botsensor2=3
.EQU	botsensorreg=PIND

; секция Кнопка старт/стоп
.EQU	buttonstart=0
.EQU	buttonstartpin=PIND


; секция ИК-бампер
.EQU	dsimpulses=10
.EQU	dswait1=19
.EQU	dswait2=23
.EQU	dswait3=100
.EQU	dsport=PORTD
.EQU	dsddr=DDRD
.EQU	dspin=PIND
.EQU	dspinin=5
.EQU	dspinout=4

; временные регистры
.DEF	temp=R30
.DEF	temp1=R29

; константы для задержек
.EQU	wait_init=$15
.EQU	wait_back=30


; scan

.equ	scan1=3
.equ	pinscan1=PIND

.equ	scan2=7
.equ	pinscan2=PINB

.equ	scan3=6
.equ	pinscan3=PINB

.equ	scan4=2
.equ	pinscan4=PIND


;====================================
;	I N I T 
;====================================
; готовим порт В 
	ldi r30,0b00011111
	out DDRB,r30

; готовим порт D ввода
	ldi r30,0
	out DDRD,r30

; инициализация стека
	ldi R30,RAMEND
	out SPL,R30

; ИК-бампер
;	cbi   dsddr, dspinin	;TSOP
;	sbi   dsddr, dspinout	
; сенсор края
;	cbi   botsensorreg, botsensor1
;	cbi   botsensorreg, botsensor2
	    
;====================================

; ожидание начатия кнопки старт
buttonstart0:
	sbic	buttonstartpin, buttonstart
	rjmp buttonstart0
; кнопка нажата- начало

init1:
; небольшая задержка перед началом движения
;	ldi r28,wait_init*2	; wait
;	rcall f_wait


;	ldi r28,10	; wait
;	rcall f_wait

; запишем в регистры начальные значения
;	ldi reg_count,motor_bcount
;	ldi reg_motor,4		; поиск

; движение вперед
	ldi temp,go_f
	out MOTORPORT,temp



start:

	sbis	buttonstartpin, buttonstart
	rjmp l_cycle


; если 4 или 1 есть 0, то нужно поворачивать, если нет, 
; проверка на наличие линии.
	
	
	
	sbis pinscan1, scan1
	rjmp l_01	
	sbis pinscan4, scan4
	rjmp l_02

	sbis pinscan2, scan2
	rjmp l_03	
	sbis pinscan3, scan3
	rjmp l_03
; stop
rjmp start


	ldi temp,0
	out MOTORPORT,temp
	rjmp l_cycle	
l_03:
	ldi temp,go_f
	out MOTORPORT,temp
	rjmp start


; если 1 и 2 или просто 1 - движение вправо
l_01:
	ldi temp,go_r
	out MOTORPORT,temp
	rjmp start

; если 3 и 4 или просто 4 - движение влево
l_02:
	ldi temp,go_l
	out MOTORPORT,temp
	rjmp start

rjmp start




l_cycle:
;	sbis	buttonstartpin, buttonstart
;	rjmp l_cycle


rjmp start
;rjmp l_cycle









;=====================================================================
;=====================================================================
;;;;;;;; подпрограмма задержки
f_wait:
	ldi r27,$1f
f_wait2:	
	
	ldi r26,$ff

f_wait3:	dec r26
	nop
	nop
	nop
	cpi r26,0
	brne f_wait3	

	dec r27
	cpi r27,0
	brne f_wait2	

	dec r28
	cpi r28,0
	brne f_wait	
	ret

	.exit	


.EQU 	go_f=$04	; оба вперед
.EQU	go_b=$08	; оба назад
.EQU	go_r=$01	; правый 
.EQU	go_l=$02	; левый 

.EQU	go_lr=$00	; левый правый
.EQU	go_rl=$00	; правый левый
.EQU	go_t=$12	; танец

.EQU	wait_turn=$07
.EQU	wait_stop=$03
.EQU	wait_back=$07


.EQU	zad2=$40	;
.EQU	zad1=$02	;

.EQU	mem1=$0070
.EQU	mem2=$0071
.EQU	mem3=$0072
.EQU	mem4=$0073
.EQU	nmem=$0074

.equ	inport=5

.include "2313def.inc"

;====================================
;	I N I T 
;====================================
; готовим порт В для вывода
	ldi r30,$FF
	out DDRB,r30


; готовим порт D для входа
	ldi r30,$00
	out DDRD,r30

; включаем резисторы- на входе будет 1
	ldi r30,$7F
	out PORTD,r30

;	cbi   DDRD, inport
;	sbi   PORTD,inport	;включаем "подтяжку"
; инициализация стека
	ldi R30,RAMEND
	out SPL,R30
;====================================

; test - попробую вставить задержку перед включением с проверкой 
; всех светодиодов
init1:

	ldi r30,$FF
	out PORTB,r30

	ldi r28,$0A	; wait
	rcall zzad		

	ldi r30,$00
	out PORTB,r30

	ldi r28,$0A	; wait
	rcall zzad		

	ldi r30,go_f
	out PORTB,r30

	ldi r28,$04	; wait
	rcall zzad		


	ldi r30,go_b
	out PORTB,r30

	ldi r28,$04	; wait
	rcall zzad		

	ldi r30,go_l
	out PORTB,r30

	ldi r28,$04	; wait
	rcall zzad		

	ldi r30,go_r
	out PORTB,r30

	ldi r28,$04	; wait
	rcall zzad		


	ldi r30,$00
	out PORTB,r30

	ldi r28,$0A	; wait
	rcall zzad		

rjmp init1

start:
; константы для задержки поиска
	ldi r30,zad1
	sts mem1, r30
	ldi r30,zad2
	sts mem2, r30
	sts mem3, r30
	sts mem4, r30

	ldi r30,go_f	;вначале- движение прямо
	out PORTB,r30	


; ждем сигнала	
loop1:

;прибавить 1 регистр 25
;	inc r25
;	lds r25,mem3
;проверить бит
;	sbrs r25,2
;	rjmp del1
;если установлен- записать движение прямо, 
;	ldi r30,go_f
;	rjmp del2
;если нет- записать 0
del1:	
;	ldi r30,0
del2:	
;	out $18,r30	
;	rjmp loopm1


	lds r30,mem4
	dec r30
	sts mem4, r30	
	cpi R30,$00
	brne loopm1
	ldi r30,zad2
	sts mem4, r30

	lds r30,mem3
	dec r30
	sts mem3, r30	
	cpi R30,$00
	brne loopm1
	ldi r30,zad2
	sts mem3, r30
	lds r30,mem2
	dec r30
	sts mem2, r30	
	cpi R30,$00
	brne loopm1
	ldi r30,zad2
	sts mem2, r30

	lds r30,mem1
	dec r30
	sts mem1, r30	
	cpi R30,$00
	breq povorotn

loopm1:
	in R30,PIND
	cpi r30,$7F
	breq loop1


;	sbis PINB, 1
;	rjmp povorotx
;	rjmp loopm1

;	sbic PORTD, inport
;	rjmp povorot_x
;	rjmp loopm1
;	breq loopm1

;	rjmp povorot_x
	

povorotx:
	lds r30,nmem
	dec r30
	sts nmem, r30	
	sbrs R30,1
	rjmp povorot_l
		
;	brne loop1

povorot_r:

	ldi r30,0 	; stop
	out $18,r30	

	ldi r28,wait_stop	; wait
	rcall zzad		

	ldi r30,go_b 	; back
	out $18,r30	

	ldi r28,wait_back	; wait
	rcall zzad		

	
	ldi r30,go_r ; поворо
	out $18,r30	

	ldi r28,wait_turn
	rcall zzad		

	ldi r30,0 	; stop
	out $18,r30	


	rjmp start

povorot_l:

	ldi r30,0 	; stop
	out $18,r30	

	ldi r28,wait_stop	; wait
	rcall zzad		

	ldi r30,go_b 	; back
	out $18,r30	

	ldi r28,wait_back	; wait
	rcall zzad		

	ldi r30,go_l ; поворо
	out $18,r30

	ldi r28,wait_turn
	rcall zzad		

	ldi r30,0 	; stop
	out $18,r30	


	rjmp start


povorotn:
	lds r30,nmem
	dec r30
	sts nmem, r30	
	sbrc R30,0
	rjmp povorot_l
	sbrc R30,1
	rjmp povorot_r
	sbrc R30,2
	rjmp povorot_t
	rjmp start

povorot_t:
	ldi r30,go_t ; поворо
	out $18,r30	

	ldi r28,wait_turn
	rcall zzad		

	rjmp start





;;;;;;;; подпрограмма задержки
zzad:
;	ldi r28,$1f
loop71:	

	ldi r27,$ff
loop72:	
	
	ldi r26,$ff

loop73:	dec r26
	nop
	nop
	nop
	cpi r26,0
	brne loop73	;---------------

	dec r27
	cpi r27,0
	brne loop72	;----------------

	dec r28
	cpi r28,0
	brne loop71	;----------------

	ret

	.exit	