.include "m8def.inc"

; секция двигателей
.EQU 	go_b= 0b00110101	; оба вперед

.EQU	go_r= 0b00111000	; правый вперед
.EQU	go_l= 0b00110010	; левый вперед

.EQU	go_f= 0b00111010	; оба назад

.EQU	go_lr=0b00111001	; левый правый
.EQU	go_rl=0b00110110	; правый левый

.EQU 	MOTORDDR=DDRC
.EQU 	MOTORPORT=PORTC
.EQU 	MOTORPIN=PINC
.EQU 	wait_motor=2
.EQU 	motor_bcount=30

.EQU	PWM01=10
.EQU	PWM02=7

.DEF 	reg_motor=R24
.DEF 	reg_count=R25



; секция Кнопка старт/стоп
.EQU	buttonstart=7
.EQU	buttonstartpin=PINB
.EQU	buttonstartddr=DDRB
.EQU	buttonstartport=PORTB

; временные регистры
.DEF	temp=R30
.DEF	temp1=R29

; константы для задержек
.EQU	wait_init=$15
.EQU	wait_back=30


; scan

.equ	scan1=6
.equ	pinscan1=PIND

.equ	scan2=7
.equ	pinscan2=PIND

.equ	scan3=0
.equ	pinscan3=PIND

.equ	scan4=1
.equ	pinscan4=PIND

.equ	scan5=4
.equ	pinscan5=PIND

.equ	scan6=5
.equ	pinscan6=PIND



;====================================
;	I N I T 
;====================================
; готовим порт В -  
	ldi r30,0b00001111
	out MOTORDDR,r30

; готовим порт D ввода
	ldi r30,0
	out DDRD,r30

; готовим порт D ввода
	ldi r30,0
	out DDRB,r30

; инициализация стека
	ldi	r30,HIGH(RAMEND) 
	out	SPH,r30	         
	ldi	r30,LOW(RAMEND)	 
	out	SPL,r30


; ИК-бампер
;	cbi   dsddr, dspinin	;TSOP
;	sbi   dsddr, dspinout	
; сенсор края
;	cbi   botsensorreg, botsensor1
;	cbi   botsensorreg, botsensor2
	    
; button
	cbi   buttonstartddr, buttonstart
	sbi   buttonstartport, buttonstart

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

; ожидание начатия кнопки старт
buttonstart0:
	sbic	buttonstartpin, buttonstart
	rjmp buttonstart0
; кнопка нажата- начало

init1:
; небольшая задержка перед началом движения
;	ldi r28,wait_init*2	; wait
;	rcall f_wait


	ldi r28,200	; wait
	rcall f_wait

; запишем в регистры начальные значения
;	ldi reg_count,motor_bcount
;	ldi reg_motor,4		; поиск

; движение вперед
	ldi temp,go_f
	out MOTORPORT,temp



;init2:
;in r30, PIND
;out PORTB, r30
;rjmp init2



start:
;rjmp start


	ldi reg_count, pwm01
wait_pwm1:
	dec reg_count
	brne wait_pwm1	
	in temp1, MOTORPIN
	ldi temp, 0
	out MOTORPORT, temp	
	ldi reg_count, pwm02
wait_pwm2:
	dec reg_count
	brne wait_pwm2
	out MOTORPORT, temp1
		
;	sbis	buttonstartpin, buttonstart
;	rjmp l_cycle

start0:
; если 4 или 1 есть 0, то нужно поворачивать, если нет, 
; проверка на наличие линии.
	
	sbis pinscan1, scan1
	rjmp l_01	
	sbis pinscan2, scan2
	rjmp l_01	

	sbis pinscan6, scan6
	rjmp l_02
	sbis pinscan5, scan5
	rjmp l_02


	sbis pinscan2, scan2
	rjmp l_03	
	sbis pinscan5, scan5
	rjmp l_03
	sbis pinscan3, scan3
	rjmp l_03	
	sbis pinscan4, scan4
	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

;	.exit	


;=====================================================================
;=====================================================================
;;;;;;;; подпрограмма задержки
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