.include "2313def.inc"
.dseg
.def temp=r30
.def temp1=r29

.cseg
  
; init 
  ldi r30,15
  out ddrb,r30

  ldi r30,15
  out portd,r30
  
  ldi r30,ramend
  out spl,r30

  cbi ddrd,4
  cbi portd,4
  sbi ddrd,5
  cbi portd,5

  sbi portb,0 ; forward
  sbi portb,2

start:
  
  ldi r28,2
  rcall s_wait

; distance sensor

  ldi temp1,10
ds00:		
  cbi portd,5
  ldi temp,19
dsdelay12:
  dec temp
  brne dsdelay12

  sbi portd,5
  ldi temp,19 
dsdelay22:
  dec temp 
  brne dsdelay22
  dec temp1
  brne ds00

  
; on-off
  sbic pind,4 
rjmp onn ;off
  cbi portb,0 ; motor off
  cbi portb,2
  ldi r28,5   ; pause for full stop
  rcall s_wait
  sbi portb,1 ; one motor rear
  ldi r28,6
  rcall s_wait 
  cbi portb,1 ; one motor stop
  ldi r28,5   ;pause for full stop
  rcall s_wait 
rjmp start  

onn:
  sbi portb,0 ; forward
  sbi portb,2
rjmp start
   
;----------------
s_wait:

s_wait1:

  ldi r27,$ff
s_wait2:

  ldi r26,$ff

s_wait3:
  dec r26
  nop
  nop
  nop
  cpi r26,0
  brne s_wait3

  dec r27
  cpi r27,0
  brne s_wait2

  dec r28
  cpi r28,0
  brne s_wait1
  ret

.exit