;-------------------------------------------------------------------------
;
;	SerialMode - Test program using MR-C3024 to drive HSR8498 servos
;				 in serial mode.
;
;				 Programmer: Fred Feucht
;				 Date:       1/29/07 
;
;
;***** Specify Device
		.device ATmega128
;-------------------------------------------------------------------------
;***** I/O Register Definitions
		.equ	UCSR1C	=$9D
		.equ	UDR1	=$9C
		.equ	UCSR1A	=$9B
		.equ	RXCIE1	= $80
		.equ	TXCIE1	= $40
		.equ	UDRIE1	= $20
		.equ	RXEN1	= $10
		.equ	TXEN1	= $08
		.equ	UCSZ12	= $04
		.equ	RXB81	= $02
		.equ	TXB81	= $01
		.equ	UCSR1B	=$9A
		.equ	UBRR1L	=$99
		.equ	UBRR1H	=$98

		.equ	SREG	=$3F
		.equ	SPH		=$3E
		.equ	SPL		=$3D

		.equ	EEARH	=$1F
		.equ	EEARL	=$1E
		.equ	EEDR	=$1D
		.equ	EECR	=$1C

		.equ	PORTA	=$1B
		.equ	DDRA	=$1A
		.equ	PINA	=$19
		.equ	PORTB	=$18
		.equ	DDRB	=$17
		.equ	PINB	=$16
		.equ	PORTC	=$15
		.equ	DDRC	=$14
		.equ	PINC	=$13
		.equ	PORTD	=$12
		.equ	DDRD	=$11
		.equ	PIND	=$10

		.def	XL	=r26
		.def	XH	=r27
		.def	YL	=r28
		.def	YH	=r29
		.def	ZL	=r30
		.def	ZH	=r31
;-------------------------------------------------------------------------
			.dseg
			.org	0x0100
Command_0:	.byte	7
Command_1:	.byte	7
Command_2:	.byte	7
Command_3:	.byte	7
Command_4:	.byte	7
Command_5:	.byte	7
Command_6:	.byte	7
Command_7:	.byte	7
Command_8:	.byte	7
Command_9:	.byte	7
Command_10:	.byte	7
Command_11:	.byte	7
Command_12:	.byte	7
Command_13:	.byte	7
Command_14:	.byte	7
Command_15:	.byte	7
Command_16:	.byte	7
Command_17:	.byte	7
Command_18:	.byte	7
Command_19:	.byte	7
Command_20:	.byte	7
Command_21:	.byte	7
Command_22:	.byte	7
Command_23:	.byte	7
			.cseg
			.org	0x0000
Mainline:
		rjmp	Start		; Jump to the start
;-------------------------------------------------------------------------
Hextab:		.db		48,49,50,51,52,53,54,55,56,57,65,66,67,68,69,70
Newline:	.db		0x0A,0x0D,0x00,0x00
MSGinit:	.db		"*** Start ***",0x00
MSGterm:	.db		"*** Done ***",0x00,0x00
MSGnull:	.db		0x20,0x00
COMM_E9:	.db		0x80, 0xE9, 0x00, 0x00, 0x97, 0x00, 0x00, 0x00
;-------------------------------------------------------------------------
Start:
		cli					; Interrupts off

		ldi		r16, 0x10	; Stack is at 0x10FF
		out		SPH, r16
		ser		r16
		out		SPL, r16
		
		ldi		YH, high(Command_0)		; move to command area
		ldi		YL, low(Command_0)

		ldi		r24, 24					; for 24 commands
MoveLoop1:
		ldi		ZH, high(COMM_E9<<1)	; move from program area
		ldi		ZL, low(COMM_E9<<1)
		
		ldi		r25, 7					; for 7 bytes
MoveLoop2:
		lpm								; copy command
		st		Y+, r0
		adiw	Z, 1		
		dec		r25
		brne	MoveLoop2
		dec		r24
		brne	MoveLoop1

		rcall	Pause125M				; Pause for servos
		rcall	Pause125M

		rcall	Command					; send commands

		rcall	Pause5M			

		rcall	Command					; send commands again

		rcall	OpenComm				; open comm port

		ldi		ZH, high(Newline<<1)	; new line
		ldi		ZL, low(Newline<<1)
		rcall	SendString

		ldi		ZH, high(MSGinit<<1)	; point to start string
		ldi		ZL, low(MSGinit<<1)
		rcall	SendString

		ldi		ZH, high(Newline<<1)	; new line
		ldi		ZL, low(Newline<<1)
		rcall	SendString

		ldi		YH, high(Command_0)		; dump Command area 	
		ldi		YL, low(Command_0)
		ldi		r16,168
		rcall	DumpData

		ldi		ZH, high(MSGterm<<1)	; point to done string
		ldi		ZL, low(MSGterm<<1)
		rcall	SendString

		ldi		ZH, high(Newline<<1)	; new line
		ldi		ZL, low(Newline<<1)
		rcall	SendString

		ldi		ZH, high(MSGnull<<1)	; point to string
		ldi		ZL, low(MSGnull<<1)
		rcall	SendString

		rcall	CloseComm				; close the comm port
Loop:
		jmp		Loop					; Loop forever
;=========================================================================
;
;	Command - send command to servos and receive response
;
;			  this subroutine clobbers every register except r29 
;
Command:
		clr		r24						; start ports low
		out		PORTA, r24
		out		PORTB, r24
		out		PORTC, r24

		ser		r24						; set ports to output
		out		DDRA, r24
		out		DDRB, r24
		out		DDRC, r24
	
		rcall	Pause52					; pause for one cycle
;
;	CommLoop1 - send first five bytes of commands
;

		clr		r24

CommLoop1:
		sbi		PORTA, 0				; set start bits
		nop
		nop
		nop
		nop
		sbi		PORTA, 1
		nop
		nop
		nop
		nop
		sbi		PORTA, 2
		nop
		nop
		nop
		nop
		sbi		PORTA, 3
		nop
		nop
		nop
		nop
		sbi		PORTA, 4
		nop
		nop
		nop
		nop
		sbi		PORTA, 5
		nop
		nop
		nop
		nop
		sbi		PORTA, 6
		nop
		nop
		nop
		nop
		sbi		PORTA, 7
		nop
		nop
		nop
		nop
		sbi		PORTB, 0
		nop
		nop
		nop
		nop
		sbi		PORTB, 1
		nop
		nop
		nop
		nop
		sbi		PORTB, 2
		nop
		nop
		nop
		nop
		sbi		PORTB, 3
		nop
		nop
		nop
		nop
		sbi		PORTB, 4
		nop
		nop
		nop
		nop
		sbi		PORTB, 5
		nop
		nop
		nop
		nop
		sbi		PORTB, 6
		nop
		nop
		nop
		nop
		sbi		PORTB, 7
		nop
		nop
		nop
		nop
		sbi		PORTC, 0
		nop
		nop
		nop
		nop
		sbi		PORTC, 1
		nop
		nop
		nop
		nop
		sbi		PORTC, 2
		nop
		nop
		nop
		nop
		sbi		PORTC, 3
		nop
		nop
		nop
		nop
		sbi		PORTC, 4
		nop
		nop
		nop
		nop
		sbi		PORTC, 5
		nop
		nop
		nop
		nop
		sbi		PORTC, 6
		nop
		nop
		nop
		nop
		sbi		PORTC, 7

		ldi		ZH, high(Command_0)		; start of command area
		ldi		ZL, low(Command_0)

		add		ZL, r24					; point to relative byte
		brsh	AddSkip1
		inc		ZH
AddSkip1:
		ld		r0,Z					; load next byte
		adiw	Z, 7
		ld		r1,Z
		adiw	Z, 7
		ld		r2,Z
		adiw	Z, 7
		ld		r3,Z
		adiw	Z, 7
		ld		r4,Z
		adiw	Z, 7
		ld		r5,Z
		adiw	Z, 7
		ld		r6,Z
		adiw	Z, 7
		ld		r7,Z
		adiw	Z, 7
		ld		r8,Z
		adiw	Z, 7
		ld		r9,Z
		adiw	Z, 7
		ld		r10,Z
		adiw	Z, 7
		ld		r11,Z
		adiw	Z, 7
		ld		r12,Z
		adiw	Z, 7
		ld		r13,Z
		adiw	Z, 7
		ld		r14,Z
		adiw	Z, 7
		ld		r15,Z
		adiw	Z, 7
		ld		r16,Z
		adiw	Z, 7
		ld		r17,Z
		adiw	Z, 7
		ld		r18,Z
		adiw	Z, 7
		ld		r19,Z
		adiw	Z, 7
		ld		r20,Z
		adiw	Z, 7
		ld		r21,Z
		adiw	Z, 7
		ld		r22,Z
		adiw	Z, 7
		ld		r23,Z

		ldi		r25, 47
Delay1:
		dec		r25
		brne	Delay1
		nop

;
;
;	CommLoop2 - shift bits and set ports
;
		ldi		r25, 8					; for 8 bits
CommLoop2:		
		ror		r0
		brlo	CommSet0
		sbi		PORTA, 0
		rjmp	CommNext0
CommSet0:
		cbi		PORTA, 0
		nop
CommNext0:
		ror		r1
		brlo	CommSet1
		sbi		PORTA, 1
		rjmp	CommNext1
CommSet1:
		cbi		PORTA, 1
		nop
CommNext1:
		ror		r2
		brlo	CommSet2
		sbi		PORTA, 2
		rjmp	CommNext2
CommSet2:
		cbi		PORTA, 2
		nop
CommNext2:
		ror		r3
		brlo	CommSet3
		sbi		PORTA, 3
		rjmp	CommNext3
CommSet3:
		cbi		PORTA, 3
		nop
CommNext3:
		ror		r4
		brlo	CommSet4
		sbi		PORTA, 4
		rjmp	CommNext4
CommSet4:
		cbi		PORTA, 4
		nop
CommNext4:
		ror		r5
		brlo	CommSet5
		sbi		PORTA, 5
		rjmp	CommNext5
CommSet5:
		cbi		PORTA, 5
		nop
CommNext5:
		ror		r6
		brlo	CommSet6
		sbi		PORTA, 6
		rjmp	CommNext6
CommSet6:
		cbi		PORTA, 6
		nop
CommNext6:
		ror		r7
		brlo	CommSet7
		sbi		PORTA, 7
		rjmp	CommNext7
CommSet7:
		cbi		PORTA, 7
		nop
CommNext7:
		ror		r8
		brlo	CommSet8
		sbi		PORTB, 0
		rjmp	CommNext8
CommSet8:
		cbi		PORTB, 0
		nop
CommNext8:
		ror		r9
		brlo	CommSet9
		sbi		PORTB, 1
		rjmp	CommNext9
CommSet9:
		cbi		PORTB, 1
		nop
CommNext9:
		ror		r10
		brlo	CommSet10
		sbi		PORTB, 2
		rjmp	CommNext10
CommSet10:
		cbi		PORTB, 2
		nop
CommNext10:
		ror		r11
		brlo	CommSet11
		sbi		PORTB, 3
		rjmp	CommNext11
CommSet11:
		cbi		PORTB, 3
		nop
CommNext11:
		ror		r12
		brlo	CommSet12
		sbi		PORTB, 4
		rjmp	CommNext12
CommSet12:
		cbi		PORTB, 4
		nop
CommNext12:
		ror		r13
		brlo	CommSet13
		sbi		PORTB, 5
		rjmp	CommNext13
CommSet13:
		cbi		PORTB, 5
		nop
CommNext13:
		ror		r14
		brlo	CommSet14
		sbi		PORTB, 6
		rjmp	CommNext14
CommSet14:
		cbi		PORTB, 6
		nop
CommNext14:
		ror		r15
		brlo	CommSet15
		sbi		PORTB, 7
		rjmp	CommNext15
CommSet15:
		cbi		PORTB, 7
		nop
CommNext15:
		ror		r16
		brlo	CommSet16
		sbi		PORTC, 0
		rjmp	CommNext16
CommSet16:
		cbi		PORTC, 0
		nop
CommNext16:
		ror		r17
		brlo	CommSet17
		sbi		PORTC, 1
		rjmp	CommNext17
CommSet17:
		cbi		PORTC, 1
		nop
CommNext17:
		ror		r18
		brlo	CommSet18
		sbi		PORTC, 2
		rjmp	CommNext18
CommSet18:
		cbi		PORTC, 2
		nop
CommNext18:
		ror		r19
		brlo	CommSet19
		sbi		PORTC, 3
		rjmp	CommNext19
CommSet19:
		cbi		PORTC, 3
		nop
CommNext19:
		ror		r20
		brlo	CommSet20
		sbi		PORTC, 4
		rjmp	CommNext20
CommSet20:
		cbi		PORTC, 4
		nop
CommNext20:
		ror		r21
		brlo	CommSet21
		sbi		PORTC, 5
		rjmp	CommNext21
CommSet21:
		cbi		PORTC, 5
		nop
CommNext21:
		ror		r22
		brlo	CommSet22
		sbi		PORTC, 6
		rjmp	CommNext22
CommSet22:
		cbi		PORTC, 6
		nop
CommNext22:
		ror		r23
		brlo	CommSet23
		sbi		PORTC, 7
		rjmp	CommNext23
CommSet23:
		cbi		PORTC, 7
		nop
CommNext23:
		
		ldi		r26, 78					; delay for necessary clocks
Delay2:	
		dec		r26
		brne	Delay2
		nop
		nop
		
		dec		r25
		breq	CommSkip1
		rjmp	CommLoop2
		nop

CommSkip1:
		cbi		PORTA, 0				; set stop bits
		nop
		nop
		nop
		nop
		cbi		PORTA, 1
		nop
		nop
		nop
		nop
		cbi		PORTA, 2
		nop
		nop
		nop
		nop
		cbi		PORTA, 3
		nop
		nop
		nop
		nop
		cbi		PORTA, 4
		nop
		nop
		nop
		nop
		cbi		PORTA, 5
		nop
		nop
		nop
		nop
		cbi		PORTA, 6
		nop
		nop
		nop
		nop
		cbi		PORTA, 7
		nop
		nop
		nop
		nop
		cbi		PORTB, 0
		nop
		nop
		nop
		nop
		cbi		PORTB, 1
		nop
		nop
		nop
		nop
		cbi		PORTB, 2
		nop
		nop
		nop
		nop
		cbi		PORTB, 3
		nop
		nop
		nop
		nop
		cbi		PORTB, 4
		nop
		nop
		nop
		nop
		cbi		PORTB, 5
		nop
		nop
		nop
		nop
		cbi		PORTB, 6
		nop
		nop
		nop
		nop
		cbi		PORTB, 7
		nop
		nop
		nop
		nop
		cbi		PORTC, 0
		nop
		nop
		nop
		nop
		cbi		PORTC, 1
		nop
		nop
		nop
		nop
		cbi		PORTC, 2
		nop
		nop
		nop
		nop
		cbi		PORTC, 3
		nop
		nop
		nop
		nop
		cbi		PORTC, 4
		nop
		nop
		nop
		nop
		cbi		PORTC, 5
		nop
		nop
		nop
		nop
		cbi		PORTC, 6
		nop
		nop
		nop
		nop
		cbi		PORTC, 7

		rcall	Pause52

		ldi		r26, 81
Delay3:
		dec		r26
		brne	Delay3

		inc		r24

		cpi		r24,5
		brsh	CommSkip2
		rjmp	CommLoop1

CommSkip2:
		sbi		PORTA, 0				; set start bits
		nop
		nop
		nop
		nop
		sbi		PORTA, 1
		nop
		nop
		nop
		nop
		sbi		PORTA, 2
		nop
		nop
		nop
		nop
		sbi		PORTA, 3
		nop
		nop
		nop
		nop
		sbi		PORTA, 4
		nop
		nop
		nop
		nop
		sbi		PORTA, 5
		nop
		nop
		nop
		nop
		sbi		PORTA, 6
		nop
		nop
		nop
		nop
		sbi		PORTA, 7
		nop
		nop
		nop
		nop
		sbi		PORTB, 0
		nop
		nop
		nop
		nop
		sbi		PORTB, 1
		nop
		nop
		nop
		nop
		sbi		PORTB, 2
		nop
		nop
		nop
		nop
		sbi		PORTB, 3
		nop
		nop
		nop
		nop
		sbi		PORTB, 4
		nop
		nop
		nop
		nop
		sbi		PORTB, 5
		nop
		nop
		nop
		nop
		sbi		PORTB, 6
		nop
		nop
		nop
		nop
		sbi		PORTB, 7
		nop
		nop
		nop
		nop
		sbi		PORTC, 0
		nop
		nop
		nop
		nop
		sbi		PORTC, 1
		nop
		nop
		nop
		nop
		sbi		PORTC, 2
		nop
		nop
		nop
		nop
		sbi		PORTC, 3
		nop
		nop
		nop
		nop
		sbi		PORTC, 4
		nop
		nop
		nop
		nop
		sbi		PORTC, 5
		nop
		nop
		nop
		nop
		sbi		PORTC, 6
		nop
		nop
		nop
		nop
		sbi		PORTC, 7
				
		clr		r24						; turn ports into inputs
		out		DDRA, r24
		out		DDRB, r24
		out		DDRC, r24

;
;	CommLoop3 - read next two bytes from servo
;
		ldi		r25, 5					; start at fifth byte
CommLoop3:
		clr		r0
		clr		r1
		clr		r2
		clr		r3
		clr		r4
		clr		r5
		clr		r6
		clr		r7
		clr		r8
		clr		r9
		clr		r10
		clr		r11
		clr		r12
		clr		r13
		clr		r14
		clr		r15
		clr		r16
		clr		r17
		clr		r18
		clr		r19
		clr		r20
		clr		r21
		clr		r22
		clr		r23

		rcall	Pause52					; pause for start bit

;
;	CommLoop4 - transfer input status to registers
;
		ldi		r24, 8
CommLoop4:
		in		r26, PINA				; get input status
		in		r27, PINB
		in		r28, PINC

		com		r26						; flip bits
		com		r27
		com		r28

		ror		r26						; shift bits into registers
		ror		r0
		ror		r26
		ror		r1
		ror		r26
		ror		r2
		ror		r26
		ror		r3
		ror		r26
		ror		r4
		ror		r26
		ror		r5
		ror		r26
		ror		r6
		ror		r26
		ror		r7
		ror		r27
		ror		r8
		ror		r27
		ror		r9
		ror		r27
		ror		r10
		ror		r27
		ror		r11
		ror		r27
		ror		r12
		ror		r27
		ror		r13
		ror		r27
		ror		r14
		ror		r27
		ror		r15
		ror		r28
		ror		r16
		ror		r28
		ror		r17
		ror		r28
		ror		r18
		ror		r28
		ror		r19
		ror		r28
		ror		r20
		ror		r28
		ror		r21
		ror		r28
		ror		r22
		ror		r28
		ror		r23

		ldi		r26, 108				; delay for necessary clocks
Delay4:
		dec		r26
		brne	Delay4
		nop
		nop

		dec		r24						; for all 8 bits
		breq	CommDone4
		rjmp	CommLoop4
		nop
CommDone4:

		ldi		ZH, high(Command_0)
		ldi		ZL, low(Command_0)

		add		ZL, r25
		brsh	AddSkip2
		inc		ZH
AddSkip2:
		st		Z, r0					; save input bytes
		adiw	Z, 7
		st		Z, r1
		adiw	Z, 7
		st		Z, r2
		adiw	Z, 7
		st		Z, r3
		adiw	Z, 7
		st		Z, r4
		adiw	Z, 7
		st		Z, r5
		adiw	Z, 7
		st		Z, r6
		adiw	Z, 7
		st		Z, r7
		adiw	Z, 7
		st		Z, r8
		adiw	Z, 7
		st		Z, r9
		adiw	Z, 7
		st		Z, r10
		adiw	Z, 7
		st		Z, r11
		adiw	Z, 7
		st		Z, r12
		adiw	Z, 7
		st		Z, r13
		adiw	Z, 7
		st		Z, r14
		adiw	Z, 7
		st		Z, r15
		adiw	Z, 7
		st		Z, r16
		adiw	Z, 7
		st		Z, r17
		adiw	Z, 7
		st		Z, r18
		adiw	Z, 7
		st		Z, r19
		adiw	Z, 7
		st		Z, r20
		adiw	Z, 7
		st		Z, r21
		adiw	Z, 7
		st		Z, r22
		adiw	Z, 7
		st		Z, r23

		ldi		r26, 93					; pause for stop bits (was 86)
Delay5:
		dec		r26
		brne	Delay5
		nop
		nop

		rcall	Pause52

		inc		r25

		cpi		r25, 7					; for two bytes
		brsh	CommDone3
		rjmp	CommLoop3

CommDone3:
		ret
;-------------------------------------------------------------------------
;
;	OpenComm - open the comm port
;
OpenComm: 
		ldi		r16,RXEN1+TXEN1	; enable the comm port
		sts		UCSR1B, r16

		ldi		r16, 0x00		; baud rate 115200
		sts		UBRR1H, r16	
		ldi		r16, 0x03		
		sts		UBRR1L, r16
		lds		r16, UDR1
		ret	
;-------------------------------------------------------------------------
;
;	CloseComm - close the comm port
;
CloseComm:
		lds		r18, UCSR1A		; wait for buffer to clear
		sbrs	r18, 5
		rjmp	CloseComm
CloseWait:
		lds		r18, UCSR1A		; wait for last character to transmit
		sbrs	r18, 6
		rjmp	CloseWait

		ldi		r16, 0x00		; disable the comm port
		sts		UCSR1B, r16

		ldi		r16, 0x00		; zero baud rate
		sts		UBRR1H, r16	
		ldi		r16, 0x00
		sts		UBRR1L, r16	
		lds		r16, UDR1
		ret	
;-------------------------------------------------------------------------
;
;	DumpData - Send hex dump of data pointed to by YH:YL for length r16
;
DumpData:
		push	ZH
		push	ZL

		ldi		r31,0x0F				; ZH = 15

		add		r16, r31				; calculate loop count
		swap	r16
		and		r16, r31
DumpLoop:
		rcall	Address					; send address and colon

		rcall	DataBlock				; send four data blocks
		rcall	DataBlock
		rcall	DataBlock
		rcall	DataBlock

		ldi		ZH, high(Newline<<1)	; new line
		ldi		ZL, low(Newline<<1)
		rcall	SendString

		dec		r16						; next line
		brne	DumpLoop

		pop		ZL
		pop		ZH
		ret
;-------------------------------------------------------------------------
;
;	Address - format address portion of dump line
;
Address:
		push	r16
		mov		r16, YH			; get high byte
		swap	r16
		andi	r16, 0x0F		; isolate high nibble
		rcall	SendHex			; send hex byte
		mov		r16, YH
		andi	r16, 0x0F		; send low nibble
		rcall	SendHex		
		mov		r16, YL			; get low bye
		swap	r16
		andi	r16, 0x0F		; isolate high nibble
		rcall	SendHex			; send hex byte
		mov		r16, YL
		andi	r16, 0x0F		; send low nibble
		rcall	SendHex		
		ldi		r16, 0x3A		; send colon (:)
		rcall	SendChar
		ldi		r16, 0x20		; send space
		rcall	SendChar
		pop		r16
		ret
;-------------------------------------------------------------------------
;
;	DataBlock - format and transmit four bytes of data and a space
;
DataBlock:
		push	r16
		push	r17
		ldi		r17, 4			; four bytes at a time
DataLoop:
		ld		r16, Y			; get data byte
		swap	r16
		andi	r16, 0x0F		; isolate high nibble
		rcall	SendHex			; send hex byte
		ld		r16, Y+
		andi	r16, 0x0F		; send low nibble
		rcall	SendHex		
		dec		r17
		brne    DataLoop
		ldi		r16, 0x20		; send space
		rcall	SendChar
		pop     r17
		pop		r16
		ret
;-------------------------------------------------------------------------
;
SendString:
		push	r0
		push	r16
SendNext:
		lpm						; text byte on r0
		adiw	Z,0x01
		tst		r0				; check for terminator
		breq	StringEnd
		mov		r16, r0
		rcall	SendChar		; transmit character
		rjmp	SendNext
StringEnd:
		pop		r16
		pop		r0
		ret	
;-------------------------------------------------------------------------
;
SendHex:
		push	r0
		ldi		ZH, high(Hextab<<1)
		ldi		ZL, low(Hextab<<1)
		add		ZL, r16
		brsh	HexAdd
		inc		ZH
HexAdd:
		lpm
		mov		r16, r0
		rcall	SendChar
		pop		r0
		ret
;-------------------------------------------------------------------------
;
SendChar:
		push	r1
SendWait:
		lds		r1, UCSR1A		; wait for last character to transmit
		sbrs	r1, 5
		rjmp	SendWait
		sts		UDR1, r16		; send character
		pop		r1
		ret
;-------------------------------------------------------------------------
;
;	Pause for 10 microseconds
;
Pause10:
		push	r24
		ldi		r24, 20
Pause10A:
		dec		r24
		brne	Pause10A
		nop
		nop
		nop
		pop		r24
		ret
;-------------------------------------------------------------------------
;
;	Pause for 45 microseconds
;
Pause45:
		push	r24
		ldi		r24, 107
Pause45A:
		dec		r24
		brne	Pause45A
		pop		r24
		ret
;-------------------------------------------------------------------------
;
;	Pause for 52 microseconds
;
Pause52:
		push	r24
		ldi		r24, 124
Pause52A:
		dec		r24
		brne	Pause52A
		pop		r24
		ret
;-------------------------------------------------------------------------
;
;	Pause for 5 milliseconds
;
Pause5M:
		push	r24
		push	r25
		ldi		r25, 48
Pause5M1:
		clr		r24
Pause5M2:
		dec		r24
		brne	Pause5M2
		dec		r25
		brne	Pause5M1
		pop		r25
		pop		r24
		ret
;-------------------------------------------------------------------------
;
;	Pause for 125 milliseconds
;
Pause125M:
		push	r24
		push	r25
		push	r26
		ldi		r24, 6
Pause125A:
		ldi		r25, 200
Pause125B:
		clr		r26
Pause125C:
		dec		r26
		brne	Pause125C
		dec		r25
		brne	Pause125B
		dec		r24
		brne	Pause125A
		pop		r26
		pop		r25
		pop		r24
		ret
;-------------------------------------------------------------------------
		.exit