;Good rectangle and satisfactory S
;Rotation right and left, movement forward and backward - OK
;Case routine - OK


.include "8515def.inc"

.def 	x0	=	r0	;First byte of distance gone by X wheel
.def 	x1	=	r1	;Second ..
.def 	y0	=	r2	;First byte of distance gone by Y wheel
.def	y1	=	r3	;Second ..
.def 	x0t	=	r4	;Temporary register of  x0
.def 	x1t	=	r5	;Temporary register of  x1
.def 	y0t	=	r6	;Temporary register of  x0
.def	y1t	=	r7	;Temporary register of  y1
.def	t4	=	r15	;delay time (seconds)

.def 	c0	=	r8	;Case routine
.def 	c1	=	r9	;Case routine

.def 	counter	=	r16
.def 	counter1	=	r17	;number of the received byte
.def 	tmp1 	=	r18	;Temporary working register
.def 	tmp2  	=	r19	;Second temporary working register
.def 	time  	=	r20	;Register is used to define delay in DelayU and DelayM subroutines
.def	clock 	=	r21	;Number of received bits
.def 	byte0	=	r22	;Temporary register
.def	t1	=	r23	;used in DelayU and DelayM subroutines
.def	t2	=	r24	;used in DelayU and DelayM subroutines
.def 	t3	=	r25	;used in DelayU and DelayM subroutines

.equ	virtual0	=	10	;virtual 0 in Move subroutine
.equ	angle	=	52	;Number of fingers for 90-degree turn
.equ	angle1	=	53	;Number of fingers for 90-degree turn

.dseg
 	char: 		.byte 1		;This byte is sended to the mouse during mouse initialisation (189)
	byte1:		.byte 1		;First received from the mouse byte
	byte2:		.byte 1		;Second received from the mouse byte
	byte3:		.byte 1		;Third received from the mouse byte
	MouseIsReady:	.byte 1		;if this byte is FFh - mouse setup is completed
	glitch:		.byte 1		;used to skip first undesirable interruption
	num1:		.byte 1		;used for different types of movement



.cseg
.org	0
	rjmp reset	
	rjmp mouse
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	

reset:	
	LDI    tmp1,low(RAMEND)	;stack setup
     	OUT    spl,tmp1		;
    	lDI    tmp1,high(RAMEND);
	OUT    sph,tmp1		;

	ldi tmp1, 189		;This will be sent during mouse initialisation
	sts char, tmp1
	
	ldi tmp1,0			;disable external interrupts
	out GIMSK,tmp1
	
	
	
	ldi tmp1,0xFF		;initialise port B as Output	
	out DDRB,tmp1		;all OUT.
	ldi tmp1,240
	out portB,tmp1		;All LEDs 

	ldi tmp1,0x00		;initialise port A as Input	
	out DDRA,tmp1		;all IN.

	ldi tmp1,0x00		;initialise port D as Input	
	out DDRD,tmp1		;all IN.

	ldi tmp1,0x00		;initialise port C as Input	
	out DDRC,tmp1		;all IN.

	
	
	ldi clock,0			;reset variables
	ldi tmp1,0			;
	mov x0,tmp1		;
	mov x1,tmp1		;
	mov x0t,tmp1		;
	mov x1t,tmp1		;
	mov y0,tmp1		;
	mov y1,tmp1		;
	mov y0t,tmp1		;
	mov y1t,tmp1		;
	mov byte0,tmp1		;
	sts MouseIsReady,tmp1	;
	sts glitch,tmp1		;
	ldi tmp1,255		;setup variables
	sts byte1,tmp1		;
	sts byte2,tmp1		;
	sts byte3,tmp1	 	;
	ldi tmp1,0xff		;
	out portA,tmp1		;
	out portD,tmp1		;
	ldi tmp1,0
	out portC,tmp1		;
	sbi DDRC,4		;
	sbi DDRC,5		;
	sbi DDRC,6		;
	sbi DDRC,7		;
	cbi DDRC,0		;
	cbi DDRC,1		;
	cbi DDRC,2		;
	cbi DDRC,3		;
	sbi portC,0		;
	sbi portC,1		;
	sbi portC,2		;
	sbi portC,3		;
	

	ldi time,100		;delay 1 s
	rcall delaym		;Power up delay.
	 
;Start of mouse initialization
	ldi tmp1,4			;set port D pin 2 as Output	
	out DDRD,tmp1		;
	cbi portD,2		;clear Clock
	ldi time,20			;
	rcall delayu		;wait 200 us 
	cbi DDRD,2		;port D pin 2 is again Input
	sbi portD,2		;
	ldi time,2			;
	rcall delayu		;wait 20 us 
	
	sbi DDRA,0		;pin 0 port A is output
	cbi portA,0		;clear Data line, waiting for clock from mouse
;
	cli
	ldi tmp1,3
	out MCUCR,tmp1		;Interruption at rising edge of Clock
	ldi tmp1,0			;
	out gifr,tmp1		;
	ldi tmp1,64		;
	out GIMSK,tmp1		;Allow external interruption 0
	sei
;End of mouse initialization
			
	ldi time,100		;delay 1 s
	rcall delaym		;Power up delay. During this time mouse sends two bytes as a confirmation of its successful reset 

	cbi DDRA,0		;pin 0 port A is input
	sbi portA,0
					
	cli			;Setup of interruption condition
	ldi tmp1,2
	out MCUCR,tmp1		;Interruption at falling edge of Clock
	ldi tmp1,0
	out gifr,tmp1
	ldi tmp1,64
	out GIMSK,tmp1
	ldi time,10			;delay 1 s
	rcall delaym		;Power up delay.
	sei	
	rjmp startIni		

startIni:				;Forever cycle "Start - goto start"
	ldi tmp1,0			;
	out DDRD,tmp1		;PortD is input


	ldi tmp1,255		;
	out portD,tmp1		;All pins are high (pull-up)

	mov tmp1,y0		;load to tmp1 y0
	out portB,tmp1

start:				;Start forever cycle
	in tmp2,pinC
	
	ldi tmp1,255		;Calculation number for case function
	eor tmp2,tmp1		;	
	ldi tmp1,15		;
	and tmp1,tmp2		;
	

	cpi tmp1,1
	brne ss1
	rjmp s01
ss1:
	cpi tmp1,2
	brne ss2
	rjmp s02
ss2:
	cpi tmp1,3
	brne ss3
	rjmp s03
ss3:
	cpi tmp1,4
	brne ss4
	rjmp s04
ss4:
	cpi tmp1,5
	brne ss5
	rjmp s05
ss5:
	cpi tmp1,6
	brne ss6
	rjmp s06
ss6:
	cpi tmp1,7
	brne ss7
	rjmp s07
ss7:
	cpi tmp1,8
	brne ss8
	rjmp s08
ss8:
	cpi tmp1,9
	brne ss9
	rjmp s09
ss9:
	cpi tmp1,10
	brne ss10
	rjmp s10
ss10:
	cpi tmp1,11
	brne ss11
	rjmp s11
ss11:
	cpi tmp1,12
	brne ss12
	rjmp s12
ss12:
	cpi tmp1,13
	brne ss13
	rjmp s13
ss13:
	cpi tmp1,14
	brne ss14
	rjmp s14
ss14:
	cpi tmp1,15
	brne ss15
	rjmp s15
ss15:
	cbi portC,4			;wheel X stop
	cbi portC,5			;
	cbi portC,6			;wheel Y stop
	cbi portC,7 			;
	
	rjmp start


s01:					;Simple movement forward without correction or any control	
	sbi portC,4			;wheel X run
	cbi portC,5			;
	cbi portC,6			;wheel Y run
	sbi portC,7 			;
	mov tmp1,x0			;load to tmp1 y0
	out portB,tmp1
	rjmp start
s02:					;Movement forward with correction and control
	ldi tmp1,0				;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,1
	mov x1,tmp1
	mov y1,tmp1
	rcall move
	ldi time,2
	rcall delays
	rjmp start
s03:	
	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,1
	mov x1,tmp1
	mov y1,tmp1
	rcall back
	ldi time,2
	rcall delays			
	rjmp start
s04:				;90 degrees rotation right
	ldi tmp1,53		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR
	ldi time,1
	rcall delays
	rjmp start
s05:				;90 degrees rotation left
	ldi tmp1,53		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnL
	ldi time,1
	rcall delays
	rjmp start
s06:
	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,2
	mov x1,tmp1
	mov y1,tmp1
	rcall move

	ldi tmp1,angle1		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,1
	mov x1,tmp1
	mov y1,tmp1
	rcall move
	
	ldi tmp1,angle1		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,2
	mov x1,tmp1
	mov y1,tmp1
	rcall move

	ldi tmp1,angle1		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,1
	mov x1,tmp1
	mov y1,tmp1
	rcall move
	
	ldi tmp1,angle1		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	rcall beep1
	ldi time,30
	rcall delaym
	rcall beep1
	ldi time,3
	rcall delays

	rjmp start
s07:
	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,2
	mov x1,tmp1
	mov y1,tmp1
	rcall move

	ldi tmp1,angle1		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,1
	mov x1,tmp1
	mov y1,tmp1
	rcall move
	
	ldi tmp1,angle1		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnL

	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,2
	mov x1,tmp1
	mov y1,tmp1
	rcall move

	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,2
	mov x1,tmp1
	mov y1,tmp1
	rcall back
	
	ldi tmp1,angle1		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,1
	mov x1,tmp1
	mov y1,tmp1
	rcall back
	
	ldi tmp1,angle1		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnL	

	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,2
	mov x1,tmp1
	mov y1,tmp1
	rcall back

	rcall beep
	ldi time,50
	rcall delaym
	rcall beep
	ldi time,3
	rcall delays

	rjmp start
s08:
	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,2
	mov x1,tmp1
	mov y1,tmp1
	rcall move

	ldi tmp1,angle		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,1
	mov x1,tmp1
	mov y1,tmp1
	rcall move
	
	ldi tmp1,angle
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,2
	mov x1,tmp1
	mov y1,tmp1
	rcall move

	ldi tmp1,angle		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	ldi tmp1,0		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,1
	mov x1,tmp1
	mov y1,tmp1
	rcall move
	
	ldi tmp1,angle		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	rcall beep1
	ldi time,100
	rcall delaym
	rcall beep1
	ldi time,3
	rcall delays

	rjmp start
s09:
	ldi tmp1,4
	sts num1,tmp1

s091:

	ldi tmp1,100		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall move
	
	ldi tmp1,angle		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnL

	ldi tmp1,100		;
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall move

	ldi tmp1,angle		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	lds tmp1,num1
	dec tmp1
	sts num1,tmp1
	cpi tmp1,0
	breq s092
	rjmp s091
	
s092:	
	ldi tmp1,angle		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR

	ldi tmp1,angle		
	mov x0,tmp1
	mov y0,tmp1
	ldi tmp1,0
	mov x1,tmp1
	mov y1,tmp1
	rcall turnR
	
	rjmp start
s10:
	rjmp start
s11:
	rjmp start
s12:
	rjmp start
s13:
	rjmp start
s14:
	rjmp start
s15:
	rjmp start


	rjmp start

turnR:					;subroutine of movement
	push tmp1 
	push tmp2
	ldi tmp1,128
	sub tmp1,x0
	mov x0,tmp1
	ldi tmp1,128
	add y0,tmp1
	
	sbi portC,4			;wheel X run
	cbi portC,5			;
	sbi portC,6			;wheel Y run
	cbi portC,7 			;
	
turnR00:
;--------------------------				;Part, which answers the question: "have we reached the finish?"
	ldi tmp1,128			;Compare less significant bytes of distance with 10
	cp x0,tmp1
	brlo turnR02 

	cbi portC,4			;wheel X run
	cbi portC,5			;	

turnR02:
	ldi tmp1,129			;Compare less significant bytes of distance with 10
	cp y0,tmp1
	brsh turnR02a			;If smaller => go on	

	cbi portC,6			;wheel X run
	cbi portC,7			;

turnR02a:
	ldi time,1
	rcall delaym
turnR01:
turnREnd:
	mov tmp1,x0			;load to tmp1 y0
	out portB,tmp1

	sbic portC,4			;If all bits are clear =>both motors are stopped. The end of the subroutine
	rjmp turnR00			;we have reached the finish
	nop		

	sbic portC,5			;	
	rjmp turnR00			;
	nop		
					;
	sbic portC,6			;
	rjmp turnR00			;
	nop				;

	sbic portC,7			;
	rjmp turnR00			;
	nop				;

	pop tmp2
	pop tmp1

	rcall beep
	ret

turnL:					;subroutine of movement
	push tmp1 
	push tmp2
	ldi tmp1,128
	sub tmp1,y0
	mov y0,tmp1
	ldi tmp1,128
	add x0,tmp1
	
	cbi portC,4			;wheel X run
	sbi portC,5			;
	cbi portC,6			;wheel Y run
	sbi portC,7 			;
	
turnL00:
;--------------------------				;Part, which answers the question: "have we reached the finish?"
	ldi tmp1,128			;Compare less significant bytes of distance with 10
	cp y0,tmp1
	brlo turnL02 

	cbi portC,6			;wheel X run
	cbi portC,7			;	
	

turnL02:
	ldi tmp1,129			;Compare less significant bytes of distance with 10
	cp x0,tmp1
	brsh turnL02a			;If smaller => go on	

	cbi portC,4			;wheel X run
	cbi portC,5			;

turnL02a:
	ldi time,1
	rcall delaym
turnL01:
	mov tmp1,x0			;load to tmp1 y0
	out portB,tmp1

	sbic portC,4			;If all bits are clear =>both of motors are stoped and it is end of subroutine
	rjmp turnL00			;we have reached the finish
	nop		

	sbic portC,5			;	
	rjmp turnL00			;
	nop		
					;
	sbic portC,6			;
	rjmp turnL00			;
	nop				;

	sbic portC,7			;
	rjmp turnL00			;
	nop				;

	pop tmp2
	pop tmp1

	rcall beep
	ret



	
copyXY:
	mov x0t,x0				;Copying registers of distance to temporary registers
	mov x1t,x1				;
	mov y0t,y0			;
	mov y1t,y1			;
	ret

clearXY:
	push tmp1

	ldi tmp1,0
	mov x0,tmp1			;
	mov x1,tmp1			;
	mov y0,tmp1			;
	mov y1,tmp1			;

	pop tmp1
	ret

move:					;subroutine of movement
	push tmp1 

	rcall copyXY			;store registers of distance in temp
	rcall clearXY			;set x0,x1,y0,y1 equal 0
	ldi tmp1,10
	mov x0,tmp1			;move to x0 10. It will be virtual 0 point	
	mov y0,tmp1			;move to y0 10. It will be virtual 0 point

	sub x0,x0t				;set registers of distance equal 0-(real distance)	
	sbc x1,x1t				;
		
	sub y0,y0t			;
	sbc y1,y1t				;


	sbi portC,4			;wheel X run
	cbi portC,5			;
	cbi portC,6			;wheel Y run
	sbi portC,7 			;
move00:
;--------------------------				;Part, which answers the question: "have we reached the finish?"
	ldi tmp1,0
	cp x1,tmp1			;Compare more significant bytes of distance with 0
	brne moveSub			;If not equal => go on movement
	cp y1,tmp1			;Compare more significant bytes of distance with 0
	brne moveSub			;If not equal => go on movement
	ldi tmp1,10
	cp x0,tmp1			;Compare less significant bytes of distance with 10
	brlo move02			;If smaller => go on	
	cbi portC,4			;wheel X stop
	cbi portC,5			;
move02:
	ldi tmp1,10
	cp y0,tmp1			;Compare less significant bytes of distance with 10
	brlo moveEnd			;if smaller => go on 
	cbi portC,6			;wheel Y stop
	cbi portC,7 			;
	rjmp moveEnd			;
;---------------------------
;===========================		;This part answer the question:"Are both wheels rotating with equal speed?" 	
moveSub:					;checking equality of distance	
	rcall copyXY			;
	sub x0t,y0t			;Subtract low bytes

	mov tmp1,x0t			;move because x0t register from r0-r15 zone
	cpi tmp1,100
	brsh moveSub01
	cpi tmp1,3				;If wheel X made more than 2 additional ticks 
	brsh moveXstop			;then stop wheel X
moveSub01:
 	cpi tmp1,254			;If wheel Y made more than 2 additional ticks 
	brlo moveYstop			;then stop wheel Y
	rjmp move01
;===========================
moveXstop:				;stop wheel X for 150 ms
	cbi portC,4			; stop wheel X
	cbi portC,5			;
	ldi time, 3
	rcall delaym
	sbi portC,4			;run wheel X
	cbi portC,5			;

	rjmp move01

moveYstop:				;stop wheel Y for 150 ms
	cbi portC,6			;stop wheel Y 
	cbi portC,7 			;
	ldi time, 3
	rcall delaym
	cbi portC,6			; run wheel Y
	sbi portC,7 			;

	rjmp move01

move01:
moveEnd:
	mov tmp1,x0			;load to tmp1 y0
	subi tmp1,10
	out portB,tmp1

	ldi time,1
	rcall delaym

	sbic portC,4			;If all bits are clear =>both motors are stopped. It is the end of subroutine
	rjmp move00			;we have reached the finish
	nop				;
	sbic portC,5			;	
	rjmp move00			;
	nop				;
	sbic portC,6			;
	rjmp move00			;
	nop				;
	sbic portC,7			;
	rjmp move00			;
	nop				;
	
	pop tmp1
	ret

moveA:					;subroutine of movement
	push tmp1 

	rcall copyXY			;store registers of distance in temp
	rcall clearXY			;set x0,x1,y0,y1 equal 0
	ldi tmp1,10
	mov x0,tmp1			;move to x0 10. It will be virtual 0 point	
	mov y0,tmp1			;move to y0 10. It will be virtual 0 point

	sub x0,x0t				;set registers of distance equal 0-(real distance)	
	sbc x1,x1t				;
	
	sub y0,y0t			;
	sbc y1,y1t				;


	sbi portC,4			;run wheel X 
	cbi portC,5			;
	cbi portC,6			;run wheel Y
	sbi portC,7 			;
moveA00:
;--------------------------				;Part, which answers the question: "have we reached the finish?"
	ldi tmp1,0
	cp x1,tmp1			;Compare more significant bytes of distance with 0
	brne moveASub			;If not equal => go on movement
	cp y1,tmp1			;Compare more significant bytes of distance with 0
	brne moveASub			;If not equal => go on movement
	ldi tmp1,10
	cp x0,tmp1			;Compare less significant bytes of distance with 10
	brlo moveA02			;If smaller => go on	
	cbi portC,4			;stop wheel X 
	cbi portC,5			;
moveA02:
	ldi tmp1,10
	cp y0,tmp1			;Compare less significant bytes of distance with 10
	brlo moveAEnd			;if smaller => go on 
	cbi portC,6			;wheel Y stop
	cbi portC,7 			;
	rjmp moveAEnd			;
;---------------------------
;===========================		;This part answer the question:"Are both wheels rotating with equal speed?" 	
moveASub:				;checking equality of distance	
	rcall copyXY			;
	sub x0t,y0t			;Subtract low bytes

	mov tmp1,x0t			;move because x0t register from r0-r15 zone
	cpi tmp1,100
	brsh moveASub01
	cpi tmp1,3				;If wheel X made more than 2 additional ticks 
	brsh moveAXstop			;then stop wheel X
moveASub01:
 	cpi tmp1,254			;If wheel Y made more than 2 additional ticks 
	brlo moveAYstop			;then stop wheel Y
	rjmp moveA01
;===========================
moveAXstop:				;stop wheel X for 150 ms
	cbi portC,4			;wheel X stop
	cbi portC,5			;
	ldi time, 3
	rcall delaym
	sbi portC,4			;wheel X run
	cbi portC,5			;

	rjmp moveA01

moveAYstop:				;stop wheel Y for 150 ms
	cbi portC,6			;stop wheel Y 
	cbi portC,7 			;
	ldi time, 3
	rcall delaym
	cbi portC,6			;run wheel Y
	sbi portC,7 			;

	rjmp moveA01

moveA01:
moveAEnd:
	mov tmp1,x0			;load to tmp1 y0
	subi tmp1,10
	out portB,tmp1

	ldi time,1
	rcall delaym

	sbic portC,4			;If all bits are clear =>both motors are stopped and it is the end of subroutine
	rjmp moveA00			;we have reached the finish
	nop				;
	sbic portC,5			;	
	rjmp moveA00			;
	nop				;
	sbic portC,6			;
	rjmp moveA00			;
	nop				;
	sbic portC,7			;
	rjmp moveA00			;
	nop				;
	
	pop tmp1
	ret



back:					;subroutine of movement
	push tmp1 

	rcall copyXY			;store registers of distance in temp
	rcall clearXY			;set x0,x1,y0,y1 equal 0
	ldi tmp1,240
	mov x0,tmp1			;move to x0 10. It will be virtual 0 point	
	mov y0,tmp1			;move to y0 10. It will be virtual 0 point

	add x0,x0t				;set registers of distance equal 0-(real distance)	
	adc x1,x1t				;
	
	add y0,y0t			;
	adc y1,y1t				;


	cbi portC,4			;wheel X run
	sbi portC,5			;
	sbi portC,6			;wheel Y run
	cbi portC,7 			;
back00:
;--------------------------				;Part which answers the question: "have we reached the finish?"
	ldi tmp1,0
	cp x1,tmp1			;Compare more significant bytes of distance with 0
	brne backSub			;If not equal => go on movement
	cp y1,tmp1			;Compare more significant bytes of distance with 0
	brne backSub			;If not equal => go on movement
	ldi tmp1,240
	cp x0,tmp1			;Compare less significant bytes of distance with 10
	brsh back02			;If smaller => go on	
	cbi portC,4			;wheel X stop
	cbi portC,5			;
back02:
	ldi tmp1,240
	cp y0,tmp1			;Compare less significant bytes of distance with 10
	brsh backEnd			;if smaller => go on 
	cbi portC,6			;wheel Y stop
	cbi portC,7 			;
	rjmp backEnd			;
;---------------------------
;===========================		;This part answer the question:"Are both wheels rotating with equal speed?" 	
backSub:					;checking equality of distance	
	rcall copyXY			;
	sub	x0t,y0t			;Subtract low bytes	sbc	
	x1t,y1t				;Add high byte with carry

	mov tmp1,x0t			;move because x0t register from r0-r15 zone
	cpi tmp1,100
	brsh backSub01
	cpi tmp1,3				;If wheel X made more than 2 additional ticks 
	brlo backXstop			;then stop wheel X
backSub01:
 	cpi tmp1,254			;If wheel Y made more than 2 additional ticks 
	brsh backYstop			;then stop wheel Y
	rjmp back01
;===========================
backXstop:				;stop wheel X for 150 ms
	cbi portC,4			;wheel X stop
	cbi portC,5			;
	ldi time, 1
	rcall delaym
	sbi portC,5			;wheel X run
	cbi portC,4			;
	rjmp back01

backYstop:				;stop wheel Y for 150 ms
	cbi portC,6			;wheel Y stop
	cbi portC,7 			;
	ldi time, 1
	rcall delaym
	cbi portC,7			;wheel Y run
	sbi portC,6 			;
	rjmp back01

back01:
backEnd:
	mov tmp1,x0			;load to tmp1 y0
	out portB,tmp1

	ldi time,1
	rcall delaym

	sbic portC,4			;If all bits are clear =>both motors are stoped and it is end of subroutine
	rjmp back00			;we have reached the finish
	nop				;
	sbic portC,5			;	
	rjmp back00			;
	nop				;
	sbic portC,6			;
	rjmp back00			;
	nop				;
	sbic portC,7			;
	rjmp back00			;
	nop				;
	
	pop tmp1
	ret





MouseSetup:

	inc clock			;Count clock
	cpi clock,3		;If clock number is less than 4, go to mouseend
	brlo MouseSetupEnd		;
	cpi clock,10		;If clock number is equal to 10, count final pulse, go to mouselast
	breq MouseSetupLast
	lds tmp1,char		
	ROR tmp1			;Rotate F4  right through Carry. Bit 0 -> C
	sts char, tmp1		;Store char back to the memory
	brcc MouseSetupClear	;If next bit is zero, go to mouseclear
	sbi portA,0		;If next bit is one, set Data line one		
	rjmp MouseSetupEnd
MouseSetupClear:
	cbi portA,0		;Because next bit is zero, clear Data line
	rjmp MouseSetupEnd
MouseSetupLast:
	ldi tmp1,255		;Set MouseIsReady equal to FF -> mouse setup complete 
	sts MouseIsReady,tmp1	;	;
	cbi DDRA,0		;pin 0 port A is input
	ldi clock,0
	
	cli			;Delay preventing reacting on first answer
	ldi time,10		;
	rcall delaym		;
	
MouseSetupEnd:
	
	pop tmp1
	reti

MouseGlitch:			;Because of some reason, there is an inintentional interruption in programm
	ldi tmp1,1			;This subroutine skip very first interruption
	sts glitch,tmp1		;
	ldi counter1,0		;Counters reset
	ldi counter,0		;
	
	pop tmp1
	reti

Mouse:
	push tmp1

	lds tmp1,MouseIsReady	;Checks if mouse initialisation was already done
	cpi tmp1,00		;
	breq MouseSetup		;If not go to mouse initialisation programm
	lds tmp1,glitch		;
	cpi tmp1,00		;Is it the very first interruption?
	breq MouseGlitch		;If yes - goto interruption skipping subroutine	
	inc counter		;Count overall number of received clocks
	cpi counter,34		;Is it 34th?
	brne Mouse00		;If not goto Mouse00	
	ldi counter,1		;If yes - new triple of bytes
	ldi counter1,0		;Reset counters
Mouse00:
	inc clock
	cpi clock,1		;If clock number is equal to 1, counted pulse is the start bit, skip.
	breq MouseEnd
	cpi clock,10		;If clock number is equal to 10, counted pulse is the parity check bit, skip.
	breq MouseEnd
	cpi clock,11		;If clock number is equal to 11, counted pulse is the last one, go and save received byte
	breq MouseLast

	in tmp1,pinA 		;Receive data from portA 
	sbic pinA,0		;If bit pinA 0 is set to 1, goto mouse mouse02
	rjmp mouse02
	nop
	clt			;clear bit T
	bld byte0,7		;load bit T to 7th bit in byte0
	rjmp mouse03
mouse02:	 
	set			;set bit T
	bld byte0,7		;load bit T to 7th bit in byte0
mouse03:
	cpi clock,9		;If clock number is equal to 9, don't need to shift right
	breq Mouse01
	lsr byte0			;Shift tmp1 right. Byte is ready to receive a new bit
mouse01:
MouseEnd:
	
	pop tmp1
	reti

MouseLast:
	ldi clock,0			;Reset clock to 0
	inc counter1		;
	cpi counter1,2		;If it was received a byte number 1 -> save it in Byte1 cell
	breq MouseByte2		;
	cpi counter1,3		;If it was received a byte number 2 -> save it in Byte1 cell
	breq MouseByte3		;Because it was byte number 3 -> save it in Byte3 cell

	sts byte1,byte0		;Save byte3
	pop tmp1
	reti
MouseByte2:			;Distance gone by X wheel
	sts byte3,byte0		;Save byte1
	cpi byte0,128		;Compare received byte with 128
	brsh MouseByte2sub	;If bigger or equal - goto MouseByte2sub. It means that we need to substitute	
	add x0,byte0		;Add new gone distance
	ldi tmp1,00		;
	adc x1,tmp1		;	
	pop tmp1
	reti
MouseByte2sub:
	ldi tmp1,255		;Here negative number is made from additional code
	sub tmp1,byte0		;
	inc tmp1		;
	mov byte0,tmp1		;Substracte from distance
	sub x0,byte0
	ldi tmp1,0
	sbc x1,tmp1
	pop tmp1
	reti

MouseByte3:			;Distance gone by Y wheel
	sts byte2,byte0		;Save byte2
	cpi byte0,128
	brsh MouseByte3sub
	add y0,byte0
	ldi tmp1,00
	adc y1,tmp1
	pop tmp1
	reti
MouseByte3sub:
	ldi tmp1,255
	sub tmp1,byte0
	inc tmp1
	mov byte0,tmp1
	sub y0,byte0
	ldi tmp1,0
	sbc y1,tmp1
	pop tmp1
	reti
	

delays:
	push t4
	mov t4,time
begin1s:
	ldi time,100
	rcall delaym
	DEC t4
	BRNE BEGIN1s
 	SEZ
	
	pop t4
	ret


;This subroutine makes delay
;delay=time*10ms
;
delaym:
	push t3
	push t2
	push t1

	mov t3,time
begin4m:
	LDI t2,250
begin3m:
	LDI t1,132
begin2m:
	DEC t1
	BRNE BEGIN2m
 	SEZ

	DEC t2
	BRNE BEGIN3m
	SEZ

	DEC t3
	BRNE BEGIN4m
	SEZ

	pop t1
	pop t2
	pop t3

	ret


;This subroutine makes delay
;delay=time*10us
;"time" - is register
;
delayu:
	
	push t3
	push t2
	push t1

	mov t2,time
begin3u:
	LDI t1,30
begin2u:
	DEC t1
	BRNE BEGIN2u
 	SEZ
;
	DEC t2
	BRNE BEGIN3u
	SEZ

	pop t1
	pop t2
	pop t3

	ret

beep:
	sbi DDRA,1
	push tmp1
	push time
	ldi tmp1,255
beepa:
	sbi portA,1
	ldi time,12
	rcall delayu
	cbi portA,1
	ldi time,12
	rcall delayu
	DEC tmp1
	BRNE beepa
 	SEZ
	
	pop time
	pop tmp1
	cbi DDRA,1
	ret

beep1:
	sbi DDRA,1
	push tmp1
	push time
	ldi tmp1,255
beep1a:
	sbi portA,1
	ldi time,16
	rcall delayu
	cbi portA,1
	ldi time,16
	rcall delayu
	DEC tmp1
	BRNE beep1a
 	SEZ
	
	pop time
	pop tmp1
	cbi DDRA,1
	ret