.include "iodefs.inc"

;***** Specify Device
		.device ATmega128
;-------------------------------------------------------------------------
			.dseg
			.org	0x0100
Command_Hi:	.byte	1
Command_Lo:	.byte	1
Dir_C:		.byte	1
Dir_B:		.byte	1
Dir_A:		.byte	1
Enable_C:	.byte	1
Enable_B:	.byte	1
Enable_A:	.byte	1
Speed:		.byte	1
Max_Hi:		.byte	1
Max_Lo:		.byte	1
Time_Hi:	.byte	1
Time_Mi:	.byte	1
Time_Lo:	.byte	1
Center:		.byte	24
MoveTemp:	.byte	24
Delta:		.byte	48
C_GetPos:	.byte	168
C_GetCandV:	.byte	168
C_MovePos:	.byte	168
C_MotorOff:	.byte	168
			.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
Msgnext:	.db		"**** Break ***",0x00,0x00
MSGnull:	.db		0x20,0x00
COMM_E9:	.db		0x80, 0xE9, 0x00, 0x00, 0x97, 0x00, 0x00, 0x00
COMM_E8:	.db		0x80, 0xE8, 0x00, 0x00, 0x98, 0x00, 0x00, 0x00
COMM_EB:	.db		0x80, 0xEB, 0x00, 0x00, 0x95, 0x00, 0x00, 0x00
COMM_EF:	.db		0x80, 0xEF, 0x00, 0x00, 0x91, 0x00, 0x00, 0x00
COMM_00:	.db		0x80, 0x00, 0x05, 0xDC, 0x9F, 0x00, 0x00, 0x00
; set servo directions
Directions:	.db		0x93, 0xF0, 0x1A, 0x00
; enabled servo flags
Enables:	.db		0xFB, 0x8E, 0x3E, 0x00		
; servo centering constants (ZEROes) 
Center_A:	.db		0, 4, -2, 0, 2, 0, 0, 5
Center_B:	.db		0, 0, 0, 0, 0, 2, -2, 0
Center_C:	.db		0, 0, 0, 4, 0, 3, 17, 0
; centering position
Zero:		.db		100, 76, 145, 93, 100, 0
			.db		100, 100, 100, 0, 0, 0
			.db		100, 100, 100, 0, 0, 0
			.db		100, 76, 145, 93, 100, 0
; standard position
Standard:	.db		100, 76, 145, 93, 100, 0
			.db		100, 30, 80, 0, 0, 0
			.db		100, 30, 80, 0, 0, 0
			.db		100, 76, 145, 93, 100, 0
;-------------------------------------------------------------------------
Start:
		cli					; Interrupts off

		ldi		r16, 0x10	; Stack is at 0x10FF
		out		SPH, r16
		ser		r16
		out		SPL, r16

		rcall	StoreInit				; Initialize storage

		rcall	CommInit				; Initialize commands		

		rcall	Pause125M				; Pause for servos
		rcall	Pause125M

		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		r24, 3
Loop1:
		push	r24

		ldi		r16, 0xF0
		rcall	SetSpeed		

		pop		r24
		dec		r24
		brne	Loop1

		ldi		ZH, high(Standard<<1)
		ldi		ZL, low(Standard<<1)
		rcall	MoveTo					; move to standard position

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

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

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

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

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

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

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

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

		ldi		YH, high(Command_Hi)	; dump Data area 	
		ldi		YL, low(Command_Hi)
		ldi		r16,110
		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

		ldi		r24, 20
Loop:
		push	r24

		ldi		ZH, high(C_GetPos)	    ; get current position
		ldi		ZL, low(C_GetPos)
		rcall	Command

		ldi		YH, high(C_GetPos)		; dump return data 	
		ldi		YL, low(C_GetPos)
		ldi		r16,168
		rcall	DumpData

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

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

		rcall	Pause125M

		pop		r24	
		dec		r24
		brne	Loop					; loop 

		rcall	CloseComm				; close the comm port
Forever:
		jmp		Forever

;=========================================================================
;
;	Command - send command to servos and receive response
;
;			  this subroutine clobbers every register except r29 
;
Command:
		sts		Command_Hi, ZH			; save command address
		sts		Command_Lo, ZL

		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

		lds		ZH, Command_Hi			; start of command area
		lds		ZL, Command_Lo

		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, 46					; necessary delay
Delay1:
		dec		r25
		brne	Delay1
		nop
		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

CommSkip1:
		nop
		nop
		nop
		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, 80					; necessary delay
Delay3:
		dec		r26
		brne	Delay3

		inc		r24

		cpi		r24,5
		brsh	CommSkip2
		rjmp	CommLoop1

CommSkip2:
		nop
		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

		ldi		r24, 7
Delay6:	
		dec		r24
		brne	Delay6
		nop
		nop

;
;	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:

		lds		ZH, Command_Hi
		lds		ZL, Command_Lo

		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, 77				; 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

;-------------------------------------------------------------------------
;
;	MoveTo - move to new positions
;
MoveTo:
		ldi		YH, high(MoveTemp)
		ldi		YL, low(MoveTemp)

		ldi		r25, 16
MoveTemp1:
		lpm		
		st		Y+, r0
		adiw	Z, 1
		dec		r25
		brne	MoveTemp1
		
		adiw	Y, 8
		ldi		r25, 8
MoveTemp2:
		lpm
		st		-Y, r0
		adiw	Z, 1
		dec		r25
		brne	MoveTemp2

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

		ldi		YH, high(C_MovePos+2)
		ldi		YL, low(C_MovePos+2)

		ldi		XH, high(Center)
		ldi		XL, low(Center)

		ldi		r17, 0x01				; r17:r16 = 500
		ldi		r16, 0xF4

		ldi		r25, 10

		ldi		r24, 24
		lds		r23, Dir_A				; get direction bits
		lds		r22, Dir_B
		lds		r21, Dir_C 
MoveLoop1:
		clr		r1
		ld		r0, Z+
		tst		r0
		brne    MoveSome				; if point = zero
		rol		r21
		rol		r22						
		rol		r23
		adiw	X, 1					; update pointers
		rjmp	MoveNone
MoveSome:
		ld		r20, X+
		clr		r19
		sub		r0, r20					; center adjust position
		sbc		r1, r19
		mul		r0, r25					; position = point x 10
		rol		r21
		rol		r22
		rol		r23
		brlo	MoveNorm
		ldi		r20, 0x07				; if direction = 1 then...
		ldi		r19, 0xD0
		sub		r19, r0					; ...position = 2000 - (point x 10)
		sbc		r20, r1
		mov		r0, r19
		mov		r1, r20 
MoveNorm:
		add		r0, r16					; position = position + 500
		adc		r1, r17
MoveNone:
		st		Y+, r1					; save new positions in command area
		st		Y+, r0
		adiw	Y, 5
		dec		r24
		brne	MoveLoop1

		ldi		XH, high(Delta)			; calculate deltas
		ldi		XL, low(Delta)

		ldi		YH, high(C_MovePos+2)
		ldi		YL, low(C_MovePos+2)

		ldi		ZH, high(C_GetPos+5)
		ldi		ZL, low(C_GetPos+5)		; find the largest move displacement

		ldi		r25, 24

		clr		r24
		
		sts		Max_Hi, r24
		sts		Max_Lo, r24
MoveMax:
		ld		r19, Y+					; get new position 
		ld		r18, Y+

		ld		r21, Z+					; get current postition
		ld		r20, Z+
		
		sub		r20, r18				; calculate displacement
		sbc		r21, r19
		brsh	MovePos

		com		r20						; twos complement result
		com		r21
		ldi		r24, 1
		add		r20, r24
		brsh	MovePos
		inc		r21
	
MovePos:		
		st		X+, r21					; save displacement
		st		X+, r20			

		lds		r19, Max_Hi				; compare to maximum so far
		lds		r18, Max_Lo
		
		sub		r18, r20
		sbc		r19, r21		
		brsh	MoveNoCarry

		sts		Max_Hi, r21				; save new maximum displacement
		sts		Max_Lo, r20
MoveNoCarry:
		adiw	Y, 5
		adiw	Z, 5

		dec		r25
		brne	MoveMax

		lds		r17, Max_Hi				; multiplicand = maximum displacement
		lds		r16, Max_Lo
		lds		r19, Speed				; multiplier = 256 - speed
		clr		r18
		sub		r18, r19
		rcall	Mul8

		sts		Time_Hi, r22			; save result
		sts		Time_Mi, r21
		sts		Time_Lo, r20	

		ldi		ZH, high(Delta)			; using displacement...
		ldi		ZL, low(Delta)

		ldi		YH, high(C_GetPos+3)	; ...calculate speed
		ldi		YL, low(C_GetPos+3)

		ldi		r21, 24
MoveSpeed:
		ld		r20, Z+					; get next displacement
		ld		r19, Z+

		lds		r16, Time_Lo			; get move time
		lds		r17, Time_Mi
		lds		r18, Time_Hi

		push	r21
		rcall	Div24					; calculate speed
		pop		r21

		ser		r19						; maximum speed	

		or		r18,r17					; check for overflow
		brne	MoveSave	

		and		r19, r16				; check for zero
		brne	MoveSave

		ldi		r19, 1
MoveSave:
		clr		r20
		sub		r20, r19
		st		Y+, r20					; save speed value

		adiw	Y, 6					; next command

		dec		r21						; for 24 commands
		brne	MoveSpeed
		
		ldi		ZH, high(C_GetPos)		; checksum speed commands
		ldi		ZL, low(C_GetPos)
		rcall	CheckSum

		ldi		ZH, high(C_GetPos)		; issue new speed commands
		ldi		ZL, low(C_GetPos)
		rcall	Command
		
		rcall	Pause5M
			
		ldi		ZH, high(C_GetPos)		; issue new speed commands
		ldi		ZL, low(C_GetPos)
		rcall	Command
		
		rcall	Pause5M

		ldi		ZH, high(C_MovePos)		; checksum move commands
		ldi		ZL, low(C_MovePos)
		rcall	CheckSum

		ldi		ZH, high(C_MovePos)		; issue move commands
		ldi		ZL, low(C_MovePos)
		rcall	Command

		ret
;-------------------------------------------------------------------------
;
;	StoreInit - initialize storage variables
;
StoreInit:
		ldi		ZH, high(Directions<<1)
		ldi		ZL, low(Directions<<1)

		lpm							; copy Dir_A	
		sts		Dir_A, r0
		adiw	Z, 1

		lpm							; copy Dir_B
		sts		Dir_B, r0
		adiw	Z, 1

		lpm							; get Dir_C
		ldi		r16, 8
DirLoop:
		ror		r0					; reverse Dir_C
		rol		r1
		dec		r16
		brne	DirLoop
		
		sts		Dir_C, r1			; save updated value


		ldi		ZH, high(Enables<<1)
		ldi		ZL, low(Enables<<1)

		lpm
		sts		Enable_A, r0		; save Enable_A
		adiw	Z, 1	

		lpm
		sts		Enable_B, r0		; save Enable_B
		adiw	Z, 1	

		lpm							; get Enable_C
		ldi		r16, 8
EnableLoop:
		ror		r0					; reverse Enable_C
		rol		r1
		dec		r16
		brne	EnableLoop
		
		sts		Enable_C, r1		; save updated value

		ldi		ZH, high(Center_A<<1)
		ldi		ZL, low(Center_A<<1)

		ldi		YH, high(Center)
		ldi		YL, low(Center)

		ldi		r16, 16
CenterLoop1:
		lpm							; copy Center_A and Center_B
		neg		r0	
		st		Y+, r0
		adiw	Z, 1
		dec		r16	
		brne	CenterLoop1

		adiw	Y, 8
		ldi		r16, 8
CenterLoop2:
		lpm							; reverse Center_C
		neg		r0	
		st		-Y, r0
		adiw	Z, 1
		dec		r16
		brne	CenterLoop2


		ret
;-------------------------------------------------------------------------
;
;	CommInit - copy commands to SRAM
;
CommInit:
		ldi		YH, high(C_MovePos)		; move to command area
		ldi		YL, low(C_MovePos)

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

		ldi		YH, high(C_MotorOff)	; move to command area
		ldi		YL, low(C_MotorOff)

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

		ldi		YH, high(C_GetPos)		; move to command area
		ldi		YL, low(C_GetPos)

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

		ldi		YH, high(C_GetCandV)		; move to command area
		ldi		YL, low(C_GetCandV)

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

		ret
;-------------------------------------------------------------------------
;
;	SetSpeed - put speed value (r16) into GetPos commands
;
SetSpeed:
		sts		Speed, r16

		ldi		ZH, high(C_GetPos+3)		; move to command area
		ldi		ZL, low(C_GetPos+3)

		ldi		r18, 0x69					; calculate checksum
		add		r18, r16
		clr		r17
		sub		r17,r18

		ldi		r18, 24
SpeedLoop:
		st		Z+, r16						; save speed value
		st		Z+, r17						; save checksum
		adiw	Z, 5						
		dec		r18
		brne	SpeedLoop

		ldi		ZH, high(C_GetPos)			; issue speed commands
		ldi		ZL, low(C_GetPos)
		rcall	Command
		
		ret

;-------------------------------------------------------------------------
;
;	Mul8 - 16x8 unsigned multiplication   
;
;	Input:  multiplier   (A:B) r17:r16 	
;           multiplicand   (C)     r18	
;
;   Output: result         r22:r21:r20
;
Mul8:
		clr		r19
		clr		r20
		clr		r21
		clr		r22
		mul		r17, r18		; A*C
		mov		r3, r1
		mov		r2, r0
		mul		r16, r18		; B*C
		add		r20, r0
		adc		r21, r1
		adc		r22, r3
		add		r21, r2
		adc		r22, r19

		ret
;-------------------------------------------------------------------------
;
;	Mul16 - 16x16 unsigned multiplication   
;
;	Input:  multiplier   (A:B) r17:r16 	
;           multiplicand (C:D) r19:r18	
;
;   Output: result             r23:r22:r21:r20
;
Mul16:
		clr		r20
		clr		r21
		clr		r22
		clr		r23
		clr		r24
		mul		r19, r17		; A*C - -
		mov		r3, r1
		mov		r2, r0
		mul		r18, r16		; - - B*D
		add		r20, r0
		adc		r21, r1
		adc		r22, r2
		adc		r23, r3
		mul		r18, r17		; - A*D - 
		add		r21, r0
		adc		r22, r1
		adc		r23, r24
		mul		r19, r16		; - B*C -
		add		r21, r0
		adc		r22, r1
		adc		r23, r24

		ret

;-------------------------------------------------------------------------
;
;	Div16 - 16/16 unsigned division   
;
;	Input:  divisor   r21:r20 
;           dividend  r19:r18	
;
;   Output: Result    r19:r18
;			Remainder r17:r16
;
Div16:
		clr		r16				; zero remainder and carry
		sub		r17, r17
		ldi		r22, 17
Div16Shift1:
		rol		r18				; shift dividend and result
		rol		r19
		dec		r22
		brne	Div16Shift2		; next bit
		ret

Div16Shift2:
		rol		r16				; shift dividend into remainder
		rol		r17
		sub		r16, r20		; remainder = remainder - divisor
		sbc		r17, r21
		brcc	Div16Neg
		add		r16, r20		; restore remainder
		adc		r17, r21
		clc						; shift zero into result
		rjmp	Div16Shift1
Div16Neg:
		sec						; shift one into result
		rjmp	Div16Shift1

;-------------------------------------------------------------------------
;
;	Mul24 - 24x16 unsigned multiplication   
;
;	Input:  multiplier   (C:B:A)          r18:r17:r16 	
;           multiplicand   (N:M)              r20:r19	
;
;   Output: result       		  r14:r13:r12:r11:r10
;
Mul24:
		clr		r15
		clr		r14
		clr		r13
		clr		r12
		clr		r11
		clr		r10
		mul		r18, r19		; - M*C - -
		mov		r3, r1
		mov		r2, r0
		mul		r16, r19		; - - - M*A
		add		r10, r0
		adc		r11, r1
		adc		r12, r2
		adc		r13, r2
		adc		r14, r15
		mul		r18, r20		; N*C - - -
		mov		r3, r1
		mov		r2, r0
		mul		r17, r19		; - - M*B -
		add		r11, r0
		adc		r12, r1
		adc		r13, r2
		adc		r14, r3
		mul		r17, r20		; - N*B - -
		add		r12, r0
		adc		r13, r1
		adc		r14, r15
		mul		r16, r20		; - - N*A -
		add		r11, r0
		adc		r12, r1
		adc		r13, r15
		adc		r14, r15

		ret

;-------------------------------------------------------------------------
;
;	Div24 - 24/16 unsigned division   
;
;	Input:  divisor   r20:r19 
;           dividend  r18:r17:r16	
;
;   Output: Result    r18:r17:r16
;			Remainder r23:r22:r21
;
Div24:
		clr		r21				; zero remainder and carry
		clr		r22
		clr		r23
		sub		r24,r24
		ldi		r25, 25
Div24A:
		rol		r16				; shift dividend and result
		rol		r17
		rol		r18
		dec		r25
		brne	Div24B			; next bit
		ret

Div24B:
		rol		r21				; shift dividend into remainder
		rol		r22
		rol		r23
		sub		r21, r19		; remainder = remainder - divisor
		sbc		r22, r20
		sbc		r23, r24
		brcc	Div24C
		add		r21, r19		; restore remainder
		adc		r22, r20
		adc		r23, r24
		clc						; shift zero into result
		rjmp	Div24A
Div24C:
		sec						; shift one into result
		rjmp	Div24A

;-------------------------------------------------------------------------
;
;	CheckSum - calculate checksum for command set
;
CheckSum:
		ldi		r16, 24
CheckLoop:
		ld		r17, Z+			; First byte
		ld		r18, Z+
		add		r17, r18		; plus second byte
		ld		r18, Z+
		add		r17, r18		; plus third byte
		ld		r18, Z+
		add		r17, r18		; plus fourth byte
		clr		r18
		sub		r18, r17		; checksum = zero minus total
		st		Z+, r18
		adiw	Z, 2			

		dec		r16				; for 24 commands
		brne	CheckLoop

		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