ltc.asm

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

;----------------------------------------------------------------------------;
; 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 (Enter tc0_ovf directly)
;	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