;----------------------------------------------------------------------------;
; Line following robot embedded program (Int.RC 8MHz)
;----------------------------------------------------------------------------;

.include "m8def.inc"	; Device definition file included in "AVR Family Assembler".
.include "avr.inc"

.def	_0	= r15	;Zero reg.

.def	_Flags	= r25	;b0: Senor capture completed
			;b1: Enable motor

.def	_Err	= r14	;Number of sensing error continue...


;-----------------------------------------------------------------------------;
; SRAM領域定義

.dseg
	.org	RAMTOP
ScanCh:	.byte	2	;Current channel and phase (for background process)

SensReg:.byte	12	;Processing registers (Value)
	.byte	12	;Processing registers (Sensor #)
	.byte	12	;Background scan register
ComVal:	.byte	12	;Inter-channel compensation value

Parms:	.byte	3	;Control parameters (PWM base, P-gain, D-gain)

Pwm:	.byte	3	;PWM control variables (Left, Right, Phase)

LineBuf:.byte	32	;Online command line buffer




;-----------------------------------------------------------------------------;
; EEPROM initial value

.eseg

eeParms:.db	0x18, 0x20, 0xF0	; PWM base, P gain, D gain
eeComVal:.byte	12			; Sensor compensation values



;-----------------------------------------------------------------------------;
; Vector table

.cseg
	rjmp	reset		; RESET
	rjmp	0		; INT0
	rjmp	0		; INT1
	rjmp	0		; TC2 COMP
	rjmp	0		; TC2 OVF
	rjmp	0		; TC1 CAPT
	rjmp	0		; TC1 COMPA
	rjmp	0		; TC1 COMPB
	rjmp	0		; TC1 OVF
;	rjmp	tc0_ovf		; TC0 OVF
;	rjmp	0		; SPI
;	rjmp	0		; USART RXC
;	rjmp	0		; USART UDRE
;	rjmp	0		; USART TXC
;	rjmp	0		; ADC
;	rjmp	0		; EE_RDY
;	rjmp	0		; ANA_COMP
;	rjmp	0		; TWI
;	rjmp	0		; SPM_RDY


;-----------------------------------------------------------------------------;
; TC0 overflow interrupt isr (9.6kHz)

tc0_ovf:
	push	AL
	outi	TCNT0, -12
	in	AL, SREG
	pushw	A
	pushw	Y
	pushw	Z

	 rcall	drive_pwm
	 rcall	scan_sens

	popw	Z
	popw	Y
	popw	A
	out	SREG, AL
	pop	AL
	reti


drive_pwm:
	in	AH, PORTB
	cbr	AH, bit1+bit2
	sbrc	_Flags, 1	;Exit if PWM disable
	rjmp	PC+3		;
	out	PORTB, AH	;
	ret			;/
	ldsw	Z, Pwm		;Load PWM values
	lds	AL, Pwm+2	;/
	cp	AL, ZL		;Left drive
	brcc	PC+2		;
	sbr	AH, bit1	;/
	cp	AL, ZH		;Right drive
	brcc	PC+2		;
	sbr	AH, bit2	;/
	out	PORTB, AH	;Set output
	subi	AL, 1		;PWM counter --;
	brcc	PC+2		;
	ldi	AL, 23		;
	sts	Pwm+2, AL	;/
	ret


scan_sens:
	ldsw	Y, ScanCh	;Get current channel pointer
	mov	ZL, YL		;
	lsl	ZL		;
	clr	ZH		;
	addiw	Z, SensReg+24	;/
	cpi	YH, 0		;Branch by channel phase
	breq	rs_0		;
	cpi	YH, 1		;
	breq	rs_1		;
	cpi	YH, 2		;
	breq	rs_2		;
	rjmp	rs_3		;/

rs_0:	inw	A, ADC		;Store base value
	stdw	Z+0, A		;/
rs_1:	inc	YH		;Next phase
	rjmp	rs_exit

rs_2:	sbi	ADCSR, ADSC	;Capture peak value
	ldi	AL, 33		;
	dec	AL		;
	brne	PC-1		;/
	outi	PORTD, 0x7f	;LED off
	inc	YH		;Next phase
	rjmp	rs_exit

rs_3:	ldd	AH, Z+0		;Store peak value - base value
	in	AL, ADCL	;
	sub	AL, AH		;
	std	Z+0, AL		;
	ldd	AH, Z+1		;
	in	AL, ADCH	;
	sbc	AL, AH		;
	std	Z+1, AL		;
	brcc	PC+3		; If result is minus, it is zero.
	std	Z+0, _0		;
	std	Z+1, _0		;/
	clr	YH		;Next cannel (0 -> 2 -> 4 -> 1 -> 3 -> 5)
	addi	YL, 2		;
	cpi	YL, 6		;
	brne	PC+3		;
	ldi	YL, 1		;
	rjmp	PC+13		;/
	brcs	PC+12		;When all channels are completed,
	sbrc	_Flags, 0	; update working regs
	rjmp	PC+9		;
	ldiw	Z, SensReg	;
	ldi	AL, 12		;
	ldd	AH, Z+24	;
	st	Z+, AH		;
	dec	AL		;
	brne	PC-3		;
	sbr	_Flags, bit0	;
	clr	YL		;/
	 rcall	sel_adch	;Capture base value
	sbi	ADCSR, ADSC	;
	ldi	AL, 33		;
	dec	AL		;
	brne	PC-1		;/
	 rcall	sel_led		;LED on
rs_exit:
	stsw	ScanCh, Y
	ret


sel_led: ;Select sensor LED specified by YL (0..5)
	mov	ZL, YL
	clr	ZH
	addiw	Z, led_pat*2
	lpm	ZL, Z
	out	PORTD, ZL
	ret

sel_adch: ;Select sensor A/D channel specified by YL (0..5)
	mov	ZL, YL
	clr	ZH
	addiw	Z, adch_pat*2
	lpm	ZL, Z
	out	ADMUX, ZL
	ret

led_pat:.db	0x7e, 0x7d, 0x7b, 0x6f, 0x5f, 0x3f
adch_pat:.db	0xc0, 0xc1, 0xc2, 0xc3, 0xc6, 0xc7




;-----------------------------------------------------------------------------;
; Power ON / Reset button released

reset:
	ldiw	A, RAMEND		;Initialize SP
	outw	SP,A			;/
	clr	_0			;Permanent zero

	ldiw	Z, RAMTOP		;Clear RAM
	clr	AL			;
	st	Z+, _0			;
	st	Z+, _0			;
	st	Z+, _0			;
	st	Z+, _0			;
	dec	AL			;
	brne	PC-5			;/

	outi	PORTB,0b11101001	;Initialize Port B
	outi	DDRB, 0b00010110	;/
	outi	PORTD, -1		;Initialize Port D
	outi	DDRD, 0b11110111	;/
	outi	PORTC, 0b00110000	;Initialize Port C

	outi	TCCR0, 0b011		;TC0.ck = 115kHz

	clr	_Flags

	sei

	ldiw	Z,m_start*2		;Start up message
	 rcall	dp_str			;/
	 rcall	load_allparms		;Load working parameters
	sbis	PINB, MOSI		;Enter online mode if cable is attached
	rjmp	monitor			;/
	rjne	enter_sleep		;Enter sleep mode if parameter is broken

;-----------------------------------------------------------------------------;
; Self Running Mode


start:
	outi	ADCSR, 0b11000101	;Enable ADC
	outi	TIMSK, (1<<TOIE0)	;Start background process

	ldiw	X, 400			;Wait for 1 sec
	 rcall	read_sens		;
	sbiw	XL, 1			;
	brne	PC-2			;/
	 rcall	read_sens		;Abort if in line sensing error
	rjne	enter_sleep		;/


; 400Hz servo processing loop

running:
	 rcall	read_sens	;Read sensor value
	breq	PC+4		; Stop if error continues for 120 times (0.3s)
	dec	_Err		;
	breq	enter_sleep	;
	rjmp	running		; /
	ldi	BL, 120		;
	mov	_Err, BL	;/

	subi	AL, low(640)	;Remove offset, 640 as center(0)
	sbci	AH, high(640)	;/
	movw	T6L, AL		;Save current value
	lds	BL, Parms+1	;--- P term
	clr	BH		;
	 rcall	muls1616	;
	movw	CL, T2L		;
	movw	DL, T4L		;/
	movw	AL, T6L		;--- D term
	sub	AL, r10		; Get difference between current and 2 times
	sbc	AH, r11		; before into AH:AL
	movw	r10, T8L	; 
	movw	T8L, T6L	; /
	ldiw	X, -127		; Clip difference between +127 and -127
	cpw	A, X		;
	brge	PC+2		;
	movw	AL, XL		;
	ldiw	X, 127		;
	cpw	A, X		;
	brlt	PC+2		;
	movw	AL, XL		; /
	lds	BL, Parms+2	;
	clr	BH		;
	 rcall	muls1616	;/
	addw	C, T2		;DL:CH = (D + P) / 512;
	adc	DL, T4L		;
	asr	DL		;
	ror	CH		;/

	lds	XH, Parms+0	;Set motor output
	mov	XL, XH		;
	sbrc	CH, 7		;
	rjmp	PC+5		;
	sub	XL, CH		;
	brcc	PC+2		;
	clr	XL		;
	rjmp	PC+5		;
	neg	CH		;
	sub	XH, CH		;
	brcc	PC+2		;
	clr	XH		;
	stsw	Pwm, X		;/
	sbr	_Flags, bit1	;Enable motor

	rjmp	running



enter_sleep:
	out	ADCSR, _0		;Disable ADC
	out	TIMSK, _0		;Stop background process
	outi	PORTB, 0b00001000	;
	outi	PORTD, -1		;
	clr	_Flags			;/
	sbis	PINB, MOSI		;Enter online mode if cable is attached
	rjmp	monitor			;/
	outi	MCUCR, 0b10100000	;Enter power-down mode
	sleep				;
	rjmp	PC-3			;/


read_sens:
	cbr	_Flags, bit0	;Wait for a scan completed
	sbrs	_Flags, 0	;
	rjmp	PC-1		;/
	 rcall	comp_sens	;Gain error compensation
	 rcall	sort_value	;Sort by strength
	 rcall	flip_value	;Flip ceiling and floor
	rjmp	get_peak	;Get peak position




;-----------------------------------------------------------------------------;
; Online Mode
; オンラインモード

monitor:
	ldiw	Z,m_crlf*2	; Display command prompt
	 rcall	dp_str		; 
	ldi	AL,'%'		; 
	 rcall	xmit		; /
	 rcall	get_line	; Get command line
	ldiw	X,LineBuf	; X -> command line
	ld	BH,X+		; BH = command char
	 rcall	caps		; /
	cpi	BH,0x0d		; Null line ?
	breq	monitor		;  yes, returen
	cpi	BH,'O'		; Output port ?
	breq	do_output	;  yes, do
	cpi	BH,'I'		; Input port ?
	breq	do_input	;  yes, do
	cpi	BH,'E'		; Edit memory ?
	rjeq	do_edit		;  yes, do
	cpi	BH,'S'		; Test motors/sensors ?
	rjeq	do_test		;  yes, do
	cpi	BH,'R'		; Run ?
	rjeq	start		;  yes, do


cmd_err:ldiw	Z,m_error*2	; Syntax error
	 rcall	dp_str		; /
	rjmp	monitor



;--------------------------------------;
; I/Oレジスタに書き込み
; Write into I/O register

do_output:
	 rcall	get_valh	; Get I/O address (0-3F)
	breq	cmd_err		; /
	andi	AL,0x3f	; I/O address in memory space (+0x20)
	mov	ZL,AL		; 
	ldi	ZH,0		; 
	adiw	ZL,0x20		; /
	 rcall	get_valh	; Get data value to be written
	breq	cmd_err		; /
	st	Z,AL		; Output data
	rjmp	monitor


;--------------------------------------;
; I/Oレジスタから読み込み
; Read from I/O register

do_input:
	ld	BH,X		; Get sub command
	 rcall	caps		; 
	mov	CL,BH		; 
	cpi	BH,'R'		; 
	brne	PC+2		; 
	inc	XL		; /

	 rcall	get_valh	; Get I/O address (0-3F)
	breq	cmd_err		; /
	andi	AL,0x3f		; I/O address on memory area (+0x20)
	mov	ZL,AL		; 
	ldi	ZH,0		; 
	adiw	ZL,0x20		; /
	ldi	AL,0x0a		; LF
	 rcall	xmit		; /
	ld	CH,Z		; 1st data
	com	CH		; /
	sbic	PINB,MOSI	; If break, exit loop
	rjmp	monitor		; /
	ld	AH,Z		; Wait data change
	cp	AH,CH		; 
	breq	PC-4		; /
	mov	CH,AH		; Display data
	 rcall	di_disp		; /
	cpi	CL,'R'
	breq	PC-8
	rjmp	monitor

di_disp:
	ldi	AL,0x0d		; CR
	 rcall	xmit		; /
	mov	AL,AH		; Hex
	 rcall	dp_byte		; /
	 rcall	space		; SP
	ldi	BH,8		; Bin
	ldi	AL,'0'
	lsl	AH
	adc	AL,ZH
	 rcall	xmit
	dec	BH
	brne	PC-5
	rcall	space		; SP
	ret


;--------------------------------------;
; メモリに書き込み
; Write into memory

do_edit:
	ld	BH,X+		; BH = command char
	 rcall	caps		; /
	cpi	BH, 'M'		;Get destination
	breq	PC+4		;
	cpi	BH, 'E'		;
	breq	PC+2		;
	rjmp	cmd_err		;
	mov	EH, BH		;/
	 rcall	get_valh	;Get memory address
	rjeq	cmd_err		;
	movw	ZL,AL		;/
	 rcall	get_valh	;Get data value to be written
	rjcs	cmd_err		;/
	breq	de_lp		;Is interactive mode?
	 rcall	wr_mem		; no, store directly and exit
	rjmp	monitor		;/

de_lp:	ldi	AL, 0x0a	; Display data prompt
	 rcall	xmit		; 
	mov	AL,ZH		; 
	 rcall	dp_byte		; 
	mov	AL,ZL		; 
	 rcall	dp_byte		; 
	 rcall	space		; 
	 rcall	rd_mem		; 
	 rcall	dp_byte		; 
	ldi	AL, '-'		; 
	 rcall	xmit		; /
	 rcall	get_line	; Input data value to be written
	ldiw	X,LineBuf	; 
	ld	AL,X		; /
	cpi	AL,'.'		; End of edit?
	rjeq	monitor		;  yes, exit.
	 rcall	get_valh	; Get data value
	rjcs	cmd_err		; /
	breq	PC+2		; Skip if blank.
	 rcall	wr_mem		; Store data into memory
	adiw	ZL,1		; Next location
	rjmp	de_lp		; Continue

rd_mem:	cpi	EH, 'E'
	brne	PC+2
	rjmp	read_eep
	ld	AL, Z
	ret
wr_mem:	cpi	EH, 'E'
	brne	PC+2
	rjmp	write_eep
	st	Z, AL
	ret


;--------------------------------------;
; Test motors and sensors

do_test:
	outi	ADCSR, 0b11000101 ;Enable ADC
	 rcall	get_valh	;secondary command (1-3)
	rjeq	cmd_err
	cpi	AL, 1		;s1 <left> <right> (Output moror PWM)
	breq	ds_1
	cpi	AL, 2		;s2 (dislpay sensor status)
	breq	ds_2
	cpi	AL, 3		;s3 (calibrate sensor)
	rjeq	do_cal
	rjmp	cmd_err

ds_1:
	 rcall	get_valh	;Left motor
	rjeq	cmd_err		;
	sts	Pwm+0, AL
	 rcall	get_valh	;Right motor
	rjeq	cmd_err		;
	sts	Pwm+1, AL

	sbr	_Flags, bit1	;Enable motor
	outi	TIMSK, (1<<TOIE0) ;Start PWM
	sbis	PINB, MOSI	;Break if any key is pressed
	rjmp	PC-1		;/
	out	TIMSK, _0	;Stop PWM
	out	PORTB, _0	;/

	rjmp	monitor


ds_2:
	cbr	_Flags, bit1	;Disable motor
	ldi	ZL, 120		;Scan 120 times
	 rcall	scan_test	;/
	sbic	PINB, MOSI	;Exit when break is detected,
	rjmp	monitor		;/
	ldi	AL, 12		;CLS
	 rcall	xmit		;/
	 rcall	comp_sens	;Compensation
	 rcall	disp_sens_regs
	 rcall	sort_value
	 rcall	flip_value
	 rcall	get_peak
	 rcall	disp_peak
	rjmp	ds_2


sort_value:
	ldiw	Y, SensReg+12	;Put sensor # to each value
	clr	AL		;
	st	Y+, AL		;
	st	Y+, _0		;
	inc	AL		;
	cpi	AL, 6		;
	brcs	PC-4		;/
	ldiw	Y, SensReg	;Sort sensor values in small order
sv_l1:	movw	ZL, YL		; (bubble sort)
sv_l2:	adiw	ZL, 2		;
	lddw	A, Y+0		;
	ldd	BL, Y+12	;
	ldd	BH, Z+0		;
	ldd	CL, Z+1		;
	ldd	CH, Z+12	;
	cp	BH, AL		;
	cpc	CL, AH		;
	brcc	PC+7		;
	std	Y+0, BH		;
	std	Y+1, CL		;
	std	Y+12, CH	;
	stdw	Z+0, A		;
	std	Z+12, BL	;
	cpi	ZL, SensReg+12-2;
	brne	sv_l2		;
	adiw	YL, 2		;
	cpi	YL, SensReg+12-2;
	brne	sv_l1		;/
	ret

flip_value:
	ldiw	Y, SensReg+6		;Get ceiling level
	clrw	A			;(= average of top 3 values)
	ld	BL, Y+			;
	ld	BH, Y+			;
	addw	A, B			;
	cpi	YL, SensReg+12		;
	brne	PC-5			;
	ldiw	B, 3			;
	 rcall	div16u			;/
	ldiw	Y, SensReg		;Flip top-bottom
	movw	T0L, AL			;
	lddw	B, Y+0			;
	subw	T0, B			;
	brcc	PC+3			;
	clrw	T0			;
	st	Y+, T0L			;
	st	Y+, T0H			;
	cpi	YL, SensReg+12		;
	brne	PC-11			;/
	ret

get_peak:
	ldiw	Y, SensReg
	lddw	C, Y+0		;Get peak value into DH:CL
	clrw	D		;
	cpi	CL, 40		;Error if peak is <40
	cpc	CH, _0		;
	retcs			;/
	clrw	T4		;Clear offset ratio
	ldd	BL, Y+12	;If peak is end of line, skip 3rd
	cpi	BL, 5		;
	breq	gp_df2		;
	cpi	BL, 0		;
	breq	gp_df2		;/
	lddw	A, Y+4		;If 3rd is less than 1/4 of peak, skip 3rd
	lslw	A		;
	lslw	A		;
	cpw	A, C		;
	brcs	gp_df2		;/
	ldd	BH, Y+16	;Error if 3rd is not next to peak
	sub	BH, BL		;
	brcc	PC+2		;
	neg	BH		;
	cpi	BH, 1		;
	retne			;/
	clrw	A		;Calcurate peak to 3rd ratio
	lddw	B, Y+4		;
	 rcall	div32u		;
	lsr	BL		;
	ror	AH		;
	sub	T4L, AH		;
	sbc	T4H, BL		;/
gp_df2:	lddw	A, Y+2		;If 2nd is less than 1/4 of peak, skip 2nd
	lslw	A		;
	lslw	A		;
	cpw	A, C		;
	brcs	gp_df3		;/
	ldd	BL, Y+12	;Error if 2nd is not next to peak
	ldd	BH, Y+14	;
	sub	BH, BL		;
	brcc	PC+2		;
	neg	BH		;
	cpi	BH, 1		;
	retne			;/
	clrw	A		;Calcurate peak to 2rd ratio
	lddw	B, Y+2		;
	 rcall	div32u		;
	lsr	BL		;
	ror	AH		;
	add	T4L, AH		;
	adc	T4H, BL		;/
gp_df3:	clr	AL		;Peak position to linear value
	ldd	AH, Y+12	;
	ldd	BL, Y+14	;
	cp	BL, AH		;
	brcc	PC+5		;
	comw	T4		;
	adc	T4L, _0		;
	adc	T4H, _0		;
	addw	A, T4		;/
	sez
	ret


disp_peak:
	breq	PC+4		;Exit if error
	ldiw	Z, m_error*2	;
	rjmp	dp_str		;/
	movw	T4L, AL
	ldi	AH, '0'		;Put scale
	mov	AL, AH		;
	 rcall	xmit		;
	ldiw	Z, m_rule1*2	;
	 rcall	dp_str		;
	inc	AH		;
	cpi	AH, '5'		;
	brne	PC-7		;
	mov	AL, AH		;
	 rcall	xmit		;
	ldiw	Z, m_crlf*2	;
	 rcall	dp_str		;/
	movw	AL, T4L		;Put marker
	ldi	BL, 32		;
	ldi	BH, 0		;
	 rcall	div16u		;
	mov	AH, AL		;
	subi	AH, 1		;
	brcs	PC+4		;
	ldi	AL, ' '		;
	 rcall	xmit		;
	rjmp	PC-4		;
	ldi	AL, '@'		;
	 rcall	xmit		;/
	ldiw	Z,m_crlf*2	;Put value
	 rcall	dp_str		;
	movw	AL, T4L		;
	ldi	CL, 5		;
	rjmp	dp_decu16	;/




disp_sens_regs:
	ldiw	Y, SensReg
ds_lp:	ldiw	Z, m_crlf*2	;CRLF
	 rcall	dp_str		;/
	ldd	AL, Y+12	;Channel#
	addi	AL, '0'		;
	 rcall	xmit		;/
	lddw	A, Y+0		;Value
	ldi	CL, 5		;
	 rcall	dp_decu16	;/
	 rcall	space		;" "
	lddw	A, Y+0		;Bar graph
	ldiw	B, 10		;
	 rcall	div16u		;
	mov	CL, AL		;
	ldi	AL, '#'		;
	 rcall	xmit		;
	subi	CL, 1		;
	brcc	PC-3		;/
	adiw	YL, 2		;Next channel
	cpi	YL, SensReg+12	;
	brne	ds_lp		;/
	ldiw	Z, m_crlf*2	;CRLF
	rjmp	dp_str		;/


scan_test:
	outi	TIMSK, (1<<TOIE0) ;Start acquisition
	cbr	_Flags, bit0	;
	sbic	PINB, MOSI	;Exit if any key is pressed
	rjmp	PC+5		;/
	sbrs	_Flags, 0	;Wait for end of a line scan
	rjmp	PC-3		;/
	dec	ZL		;Repeat ZL scans
	brne	PC-6		;/
	out	TIMSK, _0	;Stop acquisition
	outi	PORTD, -1	;/
	ret


;--------------------------------------;
; Calibration

do_cal:
	ldi	ZL, 3		;Scan 3 times
	 rcall	scan_test	;/
	 rcall	disp_sens_regs	;Display raw values

	ldiw	Y, SensReg	;Get minimum value into Z
	ldiw	Z, -1		;
	ld	AL, Y+		;
	ld	AH, Y+		;
	cpw	Z, A		;
	brcs	PC+2		;
	movw	ZL, AL		;
	cpi	YL, SensReg+12	;
	brne	PC-7		;/

	ldiw	Y, SensReg	;Create compensation table into ComVal
	clrw	A		;
	movw	BL, ZL		;
	ld	CL, Y+		;
	ld	CH, Y+		;
	clrw	D		;
	 rcall	div32u		;
	stdw	Y+34, A		;
	cpi	YL, SensReg+12	;
	brne	PC-11		;/

	 rcall	save_cal	;Save the compensation values into EEPROM

	ldi	ZL, 3		;Scan 3 times
	 rcall	scan_test	;/
	 rcall	comp_sens	;Display compensated values
	 rcall	disp_sens_regs	;/

	rjmp	monitor


comp_sens:
	ldiw	Y, SensReg
	lddw	A, Y+36	;ComVal[]
	lddw	B, Y+0	;SensReg[]
	movw	T4L, BL
	cpse	AH, _0
	 rcall	mulu1616
	st	Y+, T4L
	st	Y+, T4H
	cpi	YL, SensReg+12
	brne	PC-10
	ret


save_cal:
	ldiw	Z, eeComVal
	ldiw	Y, ComVal
	ldi	CL, 12
	rjmp	write_eeblk

save_gain:
	ldiw	Z, eeParms
	ldiw	Y, Parms
	ldi	CL, 3
	rjmp	write_eeblk

load_allparms:
	ldiw	Z, eeComVal
	ldiw	Y, ComVal
	ldi	CL, 12
	 rcall	read_eeblk
	ldiw	Z, eeParms
	ldiw	Y, Parms
	ldi	CL, 3
	rjmp	read_eeblk


write_eeblk:
	ld	AL, Y+
	 rcall	write_eep
	adiw	ZL, 1
	dec	CL
	brne	PC-4
	ret

read_eeblk:
	 rcall	read_eep
	st	Y+, AL
	adiw	ZL, 1
	dec	CL
	brne	PC-4
	ret

read_eep:
	outw	EEAR, Z
	sbi	EECR, EERE
	in	AL, EEDR
	ret

write_eep:
	outw	EEAR, Z
	out	EEDR, AL
	sbi	EECR, EEMWE
	sbi	EECR, EEWE
	sbic	EECR, EEWE
	rjmp	PC-1
	ret


;--------------------------------------;
; ホストから 1行入力
;
; Call: none
; Ret:  LineBuf

get_line:
	ldiw	X,LineBuf
rl_lp:	 rcall	receive
	st	X,AL
	cpi	AL,0x0d	; CR
	brne	PC+2
	rjmp	xmit
	cpi	AL,0x08	; BS
	brne	PC+7
	cpi	XL,low(LineBuf)
	breq	rl_lp
	 rcall	xmit
	sbiw	XL,1
	rjmp	rl_lp
	cpi	AL,' '		; SP
	brcs	rl_lp
	cpi	XL,low(LineBuf+31)
	breq	rl_lp
	 rcall	xmit
	adiw	XL,1
	rjmp	rl_lp

caps:	cpi	BH,'a'		; CAPS
	brcs	PC+2		; 
	subi	BH,0x20	; /
	ret


;--------------------------------------;
; ROMの文字列を送信
; Send ROM string
;
; Call: Z = top of the string (ASCIZ)
; Ret:  Z = next string

dp_str:	lpm	AL, Z+
	tst	AL
	brne	PC+2
	ret
	 rcall	xmit
	rjmp	PC-5


;--------------------------------------;
; display byte value in hexdecimal

dp_byte:
	swap	AL
	 rcall	PC+2
	swap	AL
	push	AL
	andi	AL,0x0f
	cpi	AL,10
	brcs	PC+2
	subi	AL,-7
	subi	AL,-0x30
	 rcall	xmit
	pop	AL
	ret

;--------------------------------------;
; display value in decimal

dp_decu8:clr	AH		;8 bit entry
dp_decu16:			;16 bit entry
	clr	T0L		;digit counter
	inc	T0L		;---- decimal string generating loop
	clr	BL		;vaT0H /= 10;
	ldi	BH, 16		;
	lslw	A		;
	rol	BL		;
	cpi	BL, 10		;
	brcs	PC+3		;
	subi	BL, 10		;
	inc	AL		;
	dec	BH		;
	brne	PC-8		;/
	subi	BL,-'0'		;Push the remander (a decimal digit)
	push	BL		;/
	cp	AL, BH		;if(var =! 0) continue digit loop;
	cpc	AH, BH		;
	brne	PC-16		;/
	cp	T0L,CL		;Adjust string length
	brcc	PC+5		;
	inc	T0L		;
	ldi	AL,' '		;
	push	AL		;
	rjmp	PC-5		;/
	pop	AL		;Put decimal string
	 rcall	xmit		;<-- Put a char (vaT0L) to memory, console or any display device
	dec	T0L		;
	brne	PC-3		;/
	ret


;--------------------------------------;
; 16bit * 16bit multiply
; 
; Multiplyer:   AH:AL
; Multiplicand: BH:BL
; Result:       T4H:T2L
; Clk:		16

muls1616:
	clr	T0L
	sbrs	AH, 7
	rjmp	PC+6
	inc	T0L
	comw	A
	adc	AL, _0
	adc	AH, _0
	sbrs	BH, 7
	rjmp	PC+6
	inc	T0L
	comw	B
	adc	BL, _0
	adc	BH, _0
	bst	T0L, 0
	rjmp	PC+2
mulu1616:
	clt
	mul	BL, AL
	movw	T2L, T0L
	mul	BH, AH
	movw	T4L, T0L
	mul	BH, AL
	add	T2H, T0L
	adc	T4L, T0H
	adc	T4H, _0
	mul	BL, AH
	add	T2H, T0L
	adc	T4L, T0H
	adc	T4H, _0

	brtc	PC+9
	comw	T2
	comw	T4
	adc	T2L, _0
	adc	T2H, _0
	adc	T4L, _0
	adc	T4H, _0
	ret


;--------------------------------------;
; 16bit / 16bit unsigned division

div16u:	clrw	T0		; Call: AH:AL = dividend
	ldi	CL,16		;       BH:BL = divisor
	lslw	A		; Ret:  AH:AL = quotient
	rolw	T0		;       T0H:T0L   = surplus
	cpw	T0,B
	brcs	PC+4
	inc	AL
	subw	T0,B
	dec	CL
	brne	PC-11
	ret


;--------------------------------------;
; 32bit / 32bit unsigned division

div32u:	clrw	T0		; Call: BH:AL = dividend
	clrw	T2		;       DH:CL = divisor
	ldi	EH,32		; Ret:  BH:AL = quotient
	lslw	A		;       T2H:T0L   = surplus
	rolw	B
	rolw	T0
	rolw	T2
	cpw	T0,C
	cpcw	T2,D
	brcs	PC+6
	inc	AL
	subw	T0,C
	sbcw	T2,D
	dec	EH
	brne	PC-19
	ret


;--------------------------------------;
; 16進数文字列の値を取得
; Get hexdecimal value
;
; Call: X = string pointer
; Ret:  X = updated
;         if    C=1: error
;         elsif Z=1: end of line, value=0
;         else:      BL:AH:AL = 24bit value

get_valh:
	ldi	BL,0
	ldiw	A,0
	ld	BH,X+
	cpi	BH,' '
	brcs	gv_n
	breq	PC-3
	rjmp	PC+2
gv_l:	ld	BH,X+
	 rcall	caps
	cpi	BH,' '+1
	brcs	gv_e
	subi	BH,'0'
	brcs	gv_x
	cpi	BH,10
	brcs	PC+4
	subi	BH,7
	cpi	BH,10
	brcs	gv_x
	cpi	BH,16
	brcc	gv_x
	swap	BH
	ori	BH,0x0f
	lsl	BH
	rolw	A
	rol	BL
	cpi	BH,0xf0
	brne	PC-5
	rjmp	gv_l
gv_x:	sec
	sez
	ret
gv_e:	sbiw	XL,1
	clc
	clz
	ret
gv_n:	sbiw	XL,1
	clc
	sez
	ret


;--------------------------------------;
; スペース表示

space:
	ldi	AL,' '
	; must be followed by transmit routine

;--------------------------------------;
; ISPケーブル使用のソフトウェアUART
; Software implemented UART using ISP cable

.equ	BPS	= 62	; Bit delay time 38.4kbps @7.4MHz (Refer to UART.ASM)
.equ	MISO	= 4	; Change these value when use at 8 or 28 pin device.
.equ	MOSI	= 3	; /

xmit:	; Transmit AL. AL is broken
	push	AH
	push	BL
	com	AL
	cli
	sec			; Start bit
	ldi	BL,10		; bit count
	ldi	AH,BPS-1	; Wait for full bit time
	dec	AH		; 
	brne	PC-1		; /
	brcs	PC+2		; MISO = bit for send
	cbi	PORTB,MISO	; 
	brcc	PC+2		; 
	sbi	PORTB,MISO	; /
	lsr	AL		; next bit
	dec	BL		; All sent ?
	brne	PC-9		;  no, coutinue
	sei
	pop	BL
	pop	AH
	ret


receive:; Receive into AL. AH is broken
	ldi	AL,0x80	; Shift register for receive
	sbic	PINB,MOSI	; Wait for rise edge of MOSI
	rjmp	PC-1		; 
	sbis	PINB,MOSI	; 
	rjmp	PC-1		; /
	ldi	AH,BPS/2	; Wait for half bit time
	dec	AH		; 
	brne	PC-1		; /
	ldi	AH,BPS		; Wait for full bit time
	dec	AH		; 
	brne	PC-1		; /
	lsr	AL		; Next bit
	sbis	PINB,MOSI	; Read MOSI into AL.7
	ori	AL,0x80	; /
	brcc	PC-6		; All bits received?  no, continue
	ret			;                     yes, end



;----------------------------------------------------------;
; ROM内文字列


m_crlf:		.db	13, 10, 0, 0
m_error:	.db	13, "???", 0, 0
m_start:	.db	13, "LTC debug monitor R0.2", 13, 10, 10, "s1 <left> <right> - Test motor", 13, 10, "s2 - Test sensor", 13, 10, "s3 - Calibrate sensor", 13, 10, "ee0 - PWM base", 13, 10, "ee1 - P gain", 13, 10, "ee2 - D gain", 13, 10, 0
m_rule1:	.db	"-------",0