ltc.asm

Source code for line followe Robot... aslo going to upload Schametic

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