;*****************************************************************************************
;Telescope Mount Controller IV
;(c) Martin Cibulski
;
;Low level motor control
;
;The motors are driven by two Allegro A3973SB microstepping drivers.
;Commands are sent to the drivers in an interrupt routine 7812.5 times per second.

;*****************************************************************************************
;	Allegro 3973 parameters

.EQU	A39_IDLE		= 0
.EQU	A39_ACTIVE		= 1

.EQU	A39_RESERVED	= 0b00

.EQU	A39_RECT_ACTIVE	= 0b00
.EQU	A39_RECT_DISABLE	= 0b01
.EQU	A39_RECT_PASSIVE	= 0b10
.EQU	A39_RECT_LOWSIDE	= 0b11

.EQU	A39_OSC_INTERN	= 0b00
.EQU	A39_OSC_EXTERN	= 0b01
.EQU	A39_OSC_EXTERN2	= 0b10
.EQU	A39_OSC_EXTERN4	= 0b11

.EQU	A39_BLANK_4	= 0b00
.EQU	A39_BLANK_6	= 0b01
.EQU	A39_BLANK_8	= 0b10
.EQU	A39_BLANK_12	= 0b11

.EQU	A39_RANGE_LOW	= 0
.EQU	A39_RANGE_HIGH	= 1

.EQU	A39_REF_INTERN	= 0
.EQU	A39_REF_EXTERN	= 1

.EQU	A39_MIXED_DECAY	= 0
.EQU	A39_SLOW_DECAY	= 3

.EQU	A39_PHASE_PLUS	= 0
.EQU	A39_PHASE_MINUS	= 1

.EQU	A39_WORD_STEP	= 0
.EQU	A39_WORD_INIT	= 1


;*****************************************************************************************
.CSEG
a39__irq_per_sec:	.db	0x40,0xBE,0x84,0x80,0x00,0x00,0x00,0x00	;7812.5 = 16 MHz / 2048

;*****************************************************************************************
;Variables
.DSEG

a39_new_params:	.byte	1

a39_offtimA:	.byte	1
a39_offtimB:	.byte	1

a39_activeA:	.byte	1
a39_activeB:	.byte	1

a39_synrecA:	.byte	1
a39_synrecB:	.byte	1

a39_dectimA:	.byte	1
a39_dectimB:	.byte	1

a39_blanktimA:	.byte	1
a39_blanktimB:	.byte	1

a39_decmodA:	.byte	1
a39_decmodB:	.byte	1

a39_rangeA:	.byte	1
a39_rangeB:	.byte	1

a39_sin_coeff3:	.byte	2
a39_sin_coeff5:	.byte	2
a39_sin_coeff7:	.byte	2

;*****************************************************************************************
;Monitor Labels
.CSEG
a39_label_tab:
	.dw	a39_motsin
	.db	L_XCMD	,"MOTSIN     "

	.dw	a39_sin_coeff3
	.db	L_WRAM	,"MOTSIN3    "

	.dw	a39_sin_coeff5
	.db	L_WRAM	,"MOTSIN5    "

	.dw	a39_sin_coeff7
	.db	L_WRAM	,"MOTSIN7    "

	.dw	a39_new_params
	.db	L_BRAM	,"MOTNEWPARM "

	.dw	a39_activeA
	.db	L_BRAM	,"MOTAACTIVE "
	.dw	a39_activeB
	.db	L_BRAM	,"MOTBACTIVE "

	.dw	a39_synrecA
	.db	L_BRAM	,"MOTASYNREC "
	.dw	a39_synrecB
	.db	L_BRAM	,"MOTBSYNREC "

	.dw	a39_offtimA
	.db	L_BRAM	,"MOTAOFFTIM "
	.dw	a39_offtimB
	.db	L_BRAM	,"MOTBOFFTIM "

	.dw	a39_dectimA
	.db	L_BRAM	,"MOTADECTIM "
	.dw	a39_dectimB
	.db	L_BRAM	,"MOTBDECTIM "

	.dw	a39_blanktimA
	.db	L_BRAM	,"MOTABLATIM "
	.dw	a39_blanktimB
	.db	L_BRAM	,"MOTBBLATIM "

	.dw	a39_decmodA
	.db	L_BRAM	,"MOTADECMOD "
	.dw	a39_decmodB
	.db	L_BRAM	,"MOTBDECMOD "

	.dw	a39_rangeA
	.db	L_BRAM	,"MOTARANGE  "
	.dw	a39_rangeB
	.db	L_BRAM	,"MOTBRANGE  "

	.dw	0
	.db	L_END	, 0

;*****************************************************************************************
;Stepper Motor Init

.CSEG
stepper_init_a3973:
	ldi	r16	,MOT_DDR_VALUE
	out	MOT_DDR	,r16
	ldi	r16	,(1<<MOT_BIT_SLEEP_A)|(1<<MOT_BIT_SLEEP_B)|(1<<MOT_BIT_STROBE)
	out	MOT_PORT	,r16

	ldi	r16	,A39_MIXED_DECAY
	sts	a39_decmodA	,r16
	sts	a39_decmodB	,r16

	ldi	r16	,A39_ACTIVE
	sts	a39_activeA	,r16
	sts	a39_activeB	,r16

	rcall	a39_send_params
	ret

;*****************************************************************************************
.CSEG
a39_motsin:
	ldi	xl	,low(a39_sin_coeff3)
	ldi	xh	,high(a39_sin_coeff3)
	ld	r0	,X+
	push	r0
	ld	r0	,X+
	push	r0
	ld	r0	,X+
	push	r0
	ld	r0	,X+
	push	r0
	ld	r0	,X+
	push	r0
	ld	r0	,X+
	push	r0

	call	next_int_value
	brtc	a39_msn_error

	sts	a39_sin_coeff3	,AKKU_7
	sts	a39_sin_coeff3+1	,AKKU_8

	call	next_int_value
	brtc	a39_msn_error

	sts	a39_sin_coeff5	,AKKU_7
	sts	a39_sin_coeff5+1	,AKKU_8

	call	next_int_value
	brtc	a39_msn_error

	sts	a39_sin_coeff7	,AKKU_7
	sts	a39_sin_coeff7+1	,AKKU_8

	pop	r0
	pop	r0
	pop	r0
	pop	r0
	pop	r0
	pop	r0
	rcall	a39_fill_sin_table
	set
	ret

a39_msn_error:
	ldi	xl	,low(a39_sin_coeff3 + 6)
	ldi	xh	,high(a39_sin_coeff3 + 6)
	pop	r0
	st	-X	,r0
	pop	r0
	st	-X	,r0
	pop	r0
	st	-X	,r0
	pop	r0
	st	-X	,r0
	pop	r0
	st	-X	,r0
	pop	r0
	st	-X	,r0
	clt
	ret


;****************************************************************************************
;Calculate Sine Table
;
;Parameters:
;****************************************************************************************

.CSEG
a39_fill_sin_table:
	.EQU	a39_mst_index	=  1	;1 byte
	.EQU	a39_mst_sin	=  2	;8 byte
	.EQU	a39_mst_maxsin	= 10	;8 byte
	.EQU	a39_mst_lspace	= 18
	LOCAL	a39_mst_lspace

	PUTS_ROM	msg_fill_sin_table

	f_const	flt__0
	f_store_l	a39_mst_maxsin

	ldi	r16	,0
	std	Y+a39_mst_index	,r16
a39_mst_loop1:
	ldd	AKKU_3	,Y+a39_mst_index
	clr	AKKU_2
	call	flt_convert_word
	f_push
	f_const	a39__1_256
	f_mul
	f_push
	f_const	flt__2pi
	f_mul
	rcall	a39_sin_series
	f_abs
	f_store_l	a39_mst_sin
	f_push
	f_push
	f_load_l	a39_mst_maxsin
	f_sub
	tst	AKKU_S
	brmi	a39_mst_01

	f_pop
	f_store_l	a39_mst_maxsin
	rjmp	a39_mst_02
a39_mst_01:
	f_pop

a39_mst_02:
	ldd	r16	,Y+a39_mst_index
	inc	r16
	std	Y+a39_mst_index	,r16
	breq	a39_mst_03

	rjmp	a39_mst_loop1
	
a39_mst_03:
	ldi	r16	,0
	std	Y+a39_mst_index	,r16
a39_mst_loop2:
	ldd	AKKU_3	,Y+a39_mst_index
	clr	AKKU_2
	call	flt_convert_word
	f_push
	f_const	a39__1_256
	f_mul
	f_push
	f_const	flt__2pi
	f_mul
	rcall	a39_sin_series
	f_push
	f_const	a39__63
	f_mul
	f_push
	f_load_l	a39_mst_maxsin
	f_div
	call	flt_double2word

	ldi	xl	,low(a39_sin_table)
	ldi	xh	,high(a39_sin_table)
	ldd	r16	,Y+a39_mst_index
	clr	r15
	add	xl	,r16
	adc	xh	,r15
	st	X	,AKKU_3

	inc	r16
	std	Y+a39_mst_index	,r16
	brne	a39_mst_loop2
	
	ENDLOCAL	a39_mst_lspace
	ret

a39__1_256:	.db	0x3F,0x70,0x00,0x00,0x00,0x00,0x00,0x00	;1/256
a39__63:	.db	0x40,0x4F,0x80,0x00,0x00,0x00,0x00,0x00	;63

.DSEG
a39_sin_table:	.byte	256

;****************************************************************************************
;fourier series
;f(x) = sin(x) + coeff3 * sin(3x) + coeff5 * sin(5x) + coeff7 * sin(7x)
;****************************************************************************************
.CSEG
a39_sin_series:
	.EQU	a39_ass_x	=  1	;
	.EQU	a39_ass_frequ	=  9	;
	.EQU	a39_ass_lspace	= 17
	LOCAL	a39_ass_lspace

	f_store_l	a39_ass_x			;x
	f_sin				;sin(x)
	f_push

	f_const	flt__3			;3
	f_store_l	a39_ass_frequ			;frequ = 3
	f_push
	f_load_l	a39_ass_x			;x
	f_mul				;3x
	f_sin				;sin(3x)
	f_push
	lds	AKKU_2	,a39_sin_coeff3
	lds	AKKU_3	,a39_sin_coeff3+1
	call	flt_convert_word
	f_mul
	f_push
	f_const	flt__0_01
	f_mul
	f_add				;sin(x) + sin(3x)
	f_push

	f_load_l	a39_ass_frequ
	f_push
	f_const	flt__2
	f_add
	f_store_l	a39_ass_frequ			;frequ = 5
	f_push
	f_load_l	a39_ass_x
	f_mul				;5x
	f_sin				;sin(5x)
	f_push
	lds	AKKU_2	,a39_sin_coeff5
	lds	AKKU_3	,a39_sin_coeff5+1
	call	flt_convert_word
	f_mul
	f_push
	f_const	flt__0_01
	f_mul
	f_add				;sin(x) + sin(3x) + sin(5x)
	f_push

	f_load_l	a39_ass_frequ
	f_push
	f_const	flt__2
	f_add
	f_store_l	a39_ass_frequ			;frequ = 7
	f_push
	f_load_l	a39_ass_x
	f_mul				;7x
	f_sin				;sin(7x)
	f_push
	lds	AKKU_2	,a39_sin_coeff7
	lds	AKKU_3	,a39_sin_coeff7+1
	call	flt_convert_word
	f_mul
	f_push
	f_const	flt__0_01
	f_mul
	f_add				;sin(x) + sin(3x) + sin(5x) + sin(7x)
		
	ENDLOCAL	a39_ass_lspace
	ret

;************************************************************
;	Timer Interrupt for Stepper Motor
;************************************************************

.CSEG

irq_timer_2:
	push	r0
	push	r1
	push	r2
	push	r3
	push	r4
	push	r5
	push	r6
	push	r7
	push	r8
	push	r9
	push	r10
	push	r11
	push	r12
	push	r13
	push	r14
	push	r15
	push	r16
	push	r17
	push	r18
	push	r19
	push	r20
	push	r21
	push	r22
	push	r23
	push	r24
	push	r25
	push	r26
	push	r27
	push	r28
	push	r29
	push	r30
	push	r31
	in	r0	,SREG
	push	r0

	call	util_minstack_check

;************************************************************
;move motor A
	ldi	yh	,high(mot_a)		;calculations for motor A
	ldi	yl	,low(mot_a)

	ldd	r2	,Y+MOT_POS		;motor position
	ldd	r3	,Y+MOT_POS+1
	ldd	r4	,Y+MOT_POS+2
	ldd	r5	,Y+MOT_POS+3
	ldd	r6	,Y+MOT_POS+4
	ldd	r7	,Y+MOT_POS+5

	mov	r14	,r5

	ldd	r8	,Y+MOT_SP		;motor speed
	ldd	r9	,Y+MOT_SP+1
	ldd	r10	,Y+MOT_SP+2
	ldd	r11	,Y+MOT_SP+3

	add	r7	,r11		;add speed to position
	adc	r6	,r10
	adc	r5	,r9
	adc	r4	,r8		;always 0x00 or 0xFF
	adc	r3	,r8
	adc	r2	,r8

	std	Y+MOT_POS	,r2		;new position
	std	Y+MOT_POS+1	,r3
	std	Y+MOT_POS+2	,r4
	std	Y+MOT_POS+3	,r5
	std	Y+MOT_POS+4	,r6
	std	Y+MOT_POS+5	,r7

;************************************************************
;move motor B
	ldi	yh	,high(mot_b)		;calculations for motor B
	ldi	yl	,low(mot_b)

	ldd	r2	,Y+MOT_POS		;motor position
	ldd	r3	,Y+MOT_POS+1
	ldd	r4	,Y+MOT_POS+2
	ldd	r5	,Y+MOT_POS+3
	ldd	r6	,Y+MOT_POS+4
	ldd	r7	,Y+MOT_POS+5

	mov	r15	,r5

	ldd	r8	,Y+MOT_SP		;motor speed
	ldd	r9	,Y+MOT_SP+1
	ldd	r10	,Y+MOT_SP+2
	ldd	r11	,Y+MOT_SP+3

	add	r7	,r11		;add speed to position
	adc	r6	,r10
	adc	r5	,r9
	adc	r4	,r8		;always 0x00 or 0xFF
	adc	r3	,r8
	adc	r2	,r8

	std	Y+MOT_POS	,r2		;new position
	std	Y+MOT_POS+1	,r3
	std	Y+MOT_POS+2	,r4
	std	Y+MOT_POS+3	,r5
	std	Y+MOT_POS+4	,r6
	std	Y+MOT_POS+5	,r7

;************************************************************
;r2	phase A1 in bit 7
;r3	current A1
;r4	phase A2 in bit 7
;r5	current A2

;r6	phase B1 in bit 7
;r7	current B1
;r8	phase B2 in bit 7
;r9	current B2

;r10	phase C1 in bit 7
;r11	current C1
;r12	phase C2 in bit 7
;r13	current C2


.CSEG
	clr	r15			;high byte for 16 bit addition
	ldi	r17	,64		;offset for cos values in sin table

;************************************************************

	lds	r1	,mot_a_pos+3		;microsteps of motor A
	ldi	xl	,low(a39_sin_table)	;sine table
	ldi	xh	,high(a39_sin_table)
	add	xl	,r1
	adc	xh	,r15
	ld	r2	,X		;sin value for coil A1

	clr	r3			;r3 = abs(r2)
	sub	r3	,r2
	brpl	mot_irq_51

	mov	r3	,r2
mot_irq_51:
	ldi	xl	,low(a39_sin_table)	;sine table
	ldi	xh	,high(a39_sin_table)
	add	r1	,r17		;add 64 to get cos value
	add	xl	,r1
	adc	xh	,r15
	ld	r4	,X		;cos value for coil A2

	clr	r5			;r5 = abs (r4)
	sub	r5	,r4
	brpl	mot_irq_52

	mov	r5	,r4
mot_irq_52:
	lds	r1	,mot_a_inverted		;if motor turns backwards
	cpse	r1	,r15
	com	r4			;change phase of coil 2

;************************************************************

	lds	r1	,mot_b_pos+3		;microsteps of motor B
	ldi	xl	,low(a39_sin_table)	;sine table
	ldi	xh	,high(a39_sin_table)
	add	xl	,r1
	adc	xh	,r15
	ld	r6	,X		;sin value for coil B1

	clr	r7			;r7 = abs(r6)
	sub	r7	,r6
	brpl	mot_irq_61

	mov	r7	,r6
mot_irq_61:

	ldi	xl	,low(a39_sin_table)	;sine table
	ldi	xh	,high(a39_sin_table)
	add	r1	,r17		;add 64 to get cos value
	add	xl	,r1
	adc	xh	,r15
	ld	r8	,X		;cos value for coil B2

	clr	r9			;r9 = abs (r8)
	sub	r9	,r8
	brpl	mot_irq_62

	mov	r9	,r8
mot_irq_62:
	lds	r1	,mot_b_inverted		;if motor turns backwards
	cpse	r1	,r15
	com	r8			;change phase of coil 2

;************************************************************

;	lds	r1	,mot_c_pos+3		;microsteps of motor C
	ldi	xl	,low(a39_sin_table)	;sine table
	ldi	xh	,high(a39_sin_table)
	add	xl	,r1
	adc	xh	,r15
	ld	r10	,X		;sin value for coil C1

	clr	r11			;r11 = abs(r10)
	sub	r11	,r10
	brpl	mot_irq_71

	mov	r11	,r10
mot_irq_71:

	ldi	xl	,low(a39_sin_table)	;sine table
	ldi	xh	,high(a39_sin_table)
	add	r1	,r17		;add 64 to get cos value
	add	xl	,r1
	adc	xh	,r15
	ld	r12	,X		;cos value for coil C2

	clr	r13			;r13 = abs (r12)
	sub	r13	,r12
	brpl	mot_irq_72

	mov	r13	,r12
mot_irq_72:

;************************************************************

	ldi	r16	,(1<<MOT_BIT_SLEEP_A)|(1<<MOT_BIT_SLEEP_B)
	out	MOT_PORT	,r16		;clear strobe, start serial transfer

	lds	r17	,a39_rangeA		;divider 4 or 8
	lds	r18	,a39_rangeB

	bst	r17	,0
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,0
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 18, range select
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	clt
	bld	r16	,MOT_BIT_DATA_A
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 17 = 0, internal reference
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	lds	r17	,a39_decmodA		;bits 1..0 for decmod
	lds	r18	,a39_decmodB

	bst	r17	,1
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,1
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 16, bridge 2 mode
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r17	,0
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,0
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 15, bridge 1 mode
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r4	,7
	bld	r16	,MOT_BIT_DATA_A
	bst	r8	,7
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 14, bridge 2 phase
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r2	,7
	bld	r16	,MOT_BIT_DATA_A
	bst	r6	,7
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 13, bridge 1 phase
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r5	,5
	bld	r16	,MOT_BIT_DATA_A
	bst	r9	,5
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 12, bridge 2 DAC MSB
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r5	,4
	bld	r16	,MOT_BIT_DATA_A
	bst	r9	,4
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 11, bridge 2 DAC bit 4
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r5	,3
	bld	r16	,MOT_BIT_DATA_A
	bst	r9	,3
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 10, bridge 2 DAC bit 3
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r5	,2
	bld	r16	,MOT_BIT_DATA_A
	bst	r9	,2
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 9, bridge 2 DAC bit 2
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r5	,1
	bld	r16	,MOT_BIT_DATA_A
	bst	r9	,1
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 8, bridge 2 DAC bit 1
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r5	,0
	bld	r16	,MOT_BIT_DATA_A
	bst	r9	,0
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 7, bridge 2 DAC LSB
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r3	,5
	bld	r16	,MOT_BIT_DATA_A
	bst	r7	,5
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 6, bridge 1 DAC MSB
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r3	,4
	bld	r16	,MOT_BIT_DATA_A
	bst	r7	,4
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 5, bridge 1 DAC bit 4
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r3	,3
	bld	r16	,MOT_BIT_DATA_A
	bst	r7	,3
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 4, bridge 1 DAC bit 3
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r3	,2
	bld	r16	,MOT_BIT_DATA_A
	bst	r7	,2
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 3, bridge 1 DAC bit 2
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r3	,1
	bld	r16	,MOT_BIT_DATA_A
	bst	r7	,1
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 2, bridge 1 DAC bit 1
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r3	,0
	bld	r16	,MOT_BIT_DATA_A
	bst	r7	,0
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 1, bridge 1 DAC LSB
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	clt
	bld	r16	,MOT_BIT_DATA_A
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;bit 0, word select = 0
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	cbi	MOT_PORT	,MOT_BIT_CLOCK
	sbi	MOT_PORT	,MOT_BIT_STROBE		;set strobe, end of transfer

;************************************************************
	lds	r16	,a39_new_params
	tst	r16
	breq	mot_irq_noparms

	rcall	a39_send_params

mot_irq_noparms:

;************************************************************
;
	pop	r0
	out	SREG	,r0

	pop	r31
	pop	r30
	pop	r29
	pop	r28
	pop	r27
	pop	r26
	pop	r25
	pop	r24
	pop	r23
	pop	r22
	pop	r21
	pop	r20
	pop	r19
	pop	r18
	pop	r17
	pop	r16
	pop	r15
	pop	r14
	pop	r13
	pop	r12
	pop	r11
	pop	r10
	pop	r9
	pop	r8
	pop	r7
	pop	r6
	pop	r5
	pop	r4
	pop	r3
	pop	r2
	pop	r1
	pop	r0
	reti



;*****************************************************************************************
a39_send_params:

	ldi	r16	,(1<<MOT_BIT_SLEEP_A)|(1<<MOT_BIT_SLEEP_B)
	out	MOT_PORT	,r16		;clear strobe, start serial transfer


	lds	r17	,a39_activeA		;active, 0 - 1
	lds	r18	,a39_activeB		;
	
	bst	r17	,0
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,0
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D18


	ldi	r16	,(1<<MOT_BIT_SLEEP_A)|(1<<MOT_BIT_SLEEP_B)
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D17 = 0, reserved bit
	cbi	MOT_PORT	,MOT_BIT_CLOCK
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D16 = 0, reserved bit


	lds	r17	,a39_synrecA		;sync rectification, 0 - 3
	lds	r18	,a39_synrecB		;active / disabled / passive / low side only

	bst	r17	,1
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,1
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D15

	bst	r17	,0
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,0
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D14


	ldi	r16	,(1<<MOT_BIT_SLEEP_A)|(1<<MOT_BIT_SLEEP_B)
	out	MOT_PORT	,r16		;select internal 4 MHz oscillator
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D13 = 0
	cbi	MOT_PORT	,MOT_BIT_CLOCK
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D12 = 0


	lds	r17	,a39_dectimA		;fast decay time, 0 - 15
	lds	r18	,a39_dectimB		;1.75 - 31.75 microseconds

	bst	r17	,3
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,3
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D11

	bst	r17	,2
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,2
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D10

	bst	r17	,1
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,1
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D9

	bst	r17	,0
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,0
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D8


	lds	r17	,a39_offtimA		;fixed off time, 0 - 31
	lds	r18	,a39_offtimB		;1.75 - 63.75 microseconds

	bst	r17	,4
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,4
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D7

	bst	r17	,3
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,3
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D6

	bst	r17	,2
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,2
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D5

	bst	r17	,1
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,1
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D4

	bst	r17	,0
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,0
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D3


	lds	r17	,a39_blanktimA		;blank time, 0 - 3
	lds	r18	,a39_blanktimB		;1.0 / 1.5 / 2.0 / 3.0 microseconds

	bst	r17	,1
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,1
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;D2
	sbi	MOT_PORT	,MOT_BIT_CLOCK

	bst	r17	,0
	bld	r16	,MOT_BIT_DATA_A
	bst	r18	,0
	bld	r16	,MOT_BIT_DATA_B
	out	MOT_PORT	,r16		;D1
	sbi	MOT_PORT	,MOT_BIT_CLOCK


	ldi	r16	,(1<<MOT_BIT_SLEEP_A)|(1<<MOT_BIT_SLEEP_B)|(1<<MOT_BIT_DATA_A)|(1<<MOT_BIT_DATA_B)
	out	MOT_PORT	,r16		;Word select = 1
	sbi	MOT_PORT	,MOT_BIT_CLOCK		;D0

	cbi	MOT_PORT	,MOT_BIT_CLOCK
	sbi	MOT_PORT	,MOT_BIT_STROBE		;set strobe, end of transfer

	lds	r16	,a39_new_params
	andi	r16	,0x02
	sts	a39_new_params	,r16

	ret

;*****************************************************************************************