call rollStop

start:
; Check to see if P3, pin 0 is ON
mov A, 	P3
orl A,	#00000001B
mov P3, A
mov A, P3
anl A, 	#00000001B
; If ON, then continue.  If not, loop to start;
jz start;

; initPause:
; wait 1s to start running
mov A, #98
call waitS

; stage1:
; forward 29"
mov A, #240 ;2
call initForward
call waitS
call rollStop
call pause

; stage2:
; cw -90
call rccw   ;L
call pause

; stage3:
; forward 53"
mov A, #110 ;2
call initForward
call waitL
call rollStop
call pause

; stage4:
; ccw 90
call rcw ;L
call pause

; stage5:
; forward 22"
mov A, #190
call initForward
call waitS

; stage8:
; forward 84"
mov A, #175 ;2
call waitL
call rollStop
call pause

; rev stage
; reverse 14"
mov A, #116
call initRev
call waitS
call rollStop
call pause

; rccw
call rcw ;L
call pause

; cross
; forward 106"
mov A, #221
call initForward
call waitL
call rollStop
call pause

; endR:
; stop running
call lockStop
mov A, #98
call waitS

waitS:
; full waitS (short) is approx 2.61s
; Setup a double counter loop
mov R2, A
wait1:
mov R0, #11111111B
mov R1, #00001010B  ; 10
wait2:
djnz r0, wait2
mov r0, #11111111B
djnz r1, wait2
djnz R2, wait1
ret

waitL:
; full waitL (long) is approx 10.4s
; Setup a double counter loop
mov R2, A
wait3:
mov R0, #11111111B
mov R1, #00101000B  ; 40
wait4:
djnz r0, wait4
mov r0, #11111111B
djnz r1, wait4
djnz R2, wait3
ret

pause:
; pause 1/2s after moving
mov A, #49
call waitS
ret

initForward:
; set forward motion
mov P1, #01010101B
ret

initRev:
; set reverse motion
mov P1, #10101010B
ret

rollStop:
; cut power
mov P1, #00000000B
ret

lockStop:
; lock motors
; currently as rollStop
mov P1, #00000000B
ret

rcw:
; rotate -90 clockwise
mov P1, #10011001B
mov A, #99 ;2
call waitS
call rollStop
ret

rccw:
; rotate 90 counterclockwise
mov P1, #01100110B
mov A, #94 ;(2)
call waitS
call rollStop
ret

end