	device          SX28, OSCXT2, TURBO, STACKX, OPTIONX
	irc_cal		IRC_SLOW
	freq            50_000_000
        reset		main
	
	org 	8
valueLB		DS 1
valueHB		DS 1
resultLB	DS 1
resultHB	DS 1

command0 	ds	1	; command encoder position LB
command1 	ds	1	; command encoder position HB
feedback0   	ds	1	; feedback encoder position LB
feedback1   	ds	1	; feedback encoder position HB
testcw0   	ds	1	; test variable
testcw1   	ds	1	; test variable
testccw0    	ds	1	; test variable
testccw1    	ds	1	; test variable
control		ds	1	; analog control of system



	watch command0,16,UDEC
	watch feedback0,16,UDEC
	watch testcw0,16,UDEC
	watch testccw0,16,UDEC
	watch control,8,UDEC

main
	mov	w, #66
	mov	command0, w
        mov	w, #1
        mov	command1, w
  	mov	w, #66
  	mov	feedback0, w
        mov	w, #0
        mov	feedback1, w

again
	break
 	clr	testcw0
	clr	testcw1
	clr	testccw0
	clr	testccw1
	clr	control

rlc_testequal
	cjne	feedback0, command0, rlc_not_equal	; command != feedback
	cjne	feedback1, command1, rlc_not_equal	; command != feedback
        jmp	rlc_equal				; command == feedback

rlc_not_equal
	mov	w, command1
       	mov	testcw1,w			
       	mov	w, command0
       	mov	testcw0, w			; testcw = command
	mov	w, feedback1
       	mov	testccw1,w
       	mov	w, feedback0
       	mov	testccw0, w			; testccw = feedback

						; if cmd - fbk < fbk-cmd then cw
	mov	w, feedback0			; testcw = cmd - fbk	
        sub	testcw0, w
	mov	w, feedback1
	sc
	movsz	w, ++feedback1
	sub	testcw1, w

	mov	w, command0			; testccw = fbk - cmd
        sub	testccw0, w
	mov	w, command1
	sc
	movsz	w, ++command1
	sub	testccw1, w

	

rlc_equal
	mov	w, #128
        mov	control, w
	jmp	again 