;-----------------------------------------------------------------
; Name:			CommandRegRead.asm
; Description:	ATMega128 Two Wire Interface (IC2) Test Program
;				Interfaces SRF09 Sonar Range Finder to IC2 Bus

; Author:		Max Koessick
; Class:		EEL5666C, Intelligent Machine Design Lab
; Date:			June 28, 2003
; Revision		1.a
; Changes to Date:
;				6/28/03 First Revision
;-----------------------------------------------------------------

.nolist								; Do not include in .lst file
.include "m128def.inc"				; Standard ATMega128 Include File
.list
; Interrupt service vectors

.org $0000
	rjmp Reset						; Reset vector
;.org INT0addr
;	rjmp IntV0						; INT0 vector (ext. interrupt from pin D0)	
;.org INT1addr	
;	rjmp IntV1						; INT1 vector (ext. interrupt from pin D1)
;.org INT2addr		
;	rjmp IntV2						; INT2 vector (ext. interrupt from pin D2)


	
;-----------------------------------------------------------------
; Register defines for main loop
;-----------------------------------------------------------------


.def		mpr		=r16			; defines multipurpose register
.def		mpr2	=r17			; multipurpose register 2
.def		ECHOL	=r18
.def		ECHOH	=r19
.def		ErrorReg=r20
.def		mpr3	=r21


; Equate statements
.equ		start		= $08		; Start Condition 
.equ		Rep_start	= $10		; Repeated Start Condition Message
.equ		MT_SLA_ACK	= $18		; Master Transmitter Slave Address Acknowledge
.equ		MT_DATA_ACK = $28		; Master Transmitter Data Acknowledge
.equ		MT_DATA_NACK= $30		; Master Transmitter Data Not Acknowledge
.equ		MR_SLA_ACK	= $40		; Master Receiver Slave Address Acknowledge
.equ		MR_SLA_NACK	= $48		; Master Receiver Slave Address Acknowledge
.equ		MR_DATA_ACK = $50		; Master Receiver Data Acknowlede
.equ		MR_DATA_NACK= $58		; Master Receiver Data Not Acknowledge
.equ		W			= 0			; Write Bit
.equ		R			= 1			; Read Bit
.equ		SLA			= $A0		; Slave Address of Sonar Ranger SRF08
.equ		Inches		= $50		; Return result in Inches
.equ		CommandReg	= $00		; SRF08 Command Register
;-----------------------------------------------------------------
; Reset vector
;-----------------------------------------------------------------

Reset:
;-----Setting Stackpointer----------------------------------------
	ldi		MPR,low(RAMEND)			; Set stackptr to ram end
	out		SPL,MPR
	ldi 	MPR, high(RAMEND)
	out 	SPH, MPR

;-----Set Port Directions-----------------------------------------
	ser		mpr						; Set TEMP to $FF to...

	out 	DDRC,mpr
	ser		mpr
	out		PORTC,mpr				; Lights off

	ldi		mpr,$01
	out		DDRD,mpr				; Set PD0 as output and PD1 as input

;	ldi		mpr,(1<<PD0)			; Enable Pullup on SCL Output
;	out		PORTD,mpr

;-----------------------------------------------------------------

	clr 	ErrorReg				
	

; Set TWIBitRate for fclk=16Mhz

	ldi		mpr,12					;400Khz=16MHz/(16+2*12) See Datasheet Pg202
	sts		TWBR,mpr

	ldi		mpr,$00
	sts		TWSR,mpr

; Initialize TWCR Register

	ldi 		MPR, (1<<TWINT)|(1<<TWEA) | (1<<TWEN);
	sts	 		TWCR,MPR 			; Send START condition
	sei								; set interrupts active
			
; **MASTER TRANSMITTER*****


	ldi 		MPR, (1<<TWINT)|(1<<TWSTA)|(1<<TWEN)|(0<<TWSTO);
	sts	 		TWCR,MPR 				; Send START condition


WAIT1: 
	lds 		MPR,TWCR			; Wait for TWINT Flag set. This indicates that
	sbrs 		MPR,TWINT 			; the START condition has been transmitted
	rjmp 		WAIT1

	lds 		MPR,TWSR 			; Check value of TWI Status Register.
	andi		MPR,$F8				; Mask Prescaler Bits
	cpi 		MPR,START 			; If status different from START go to ERROR
	breq		NEXT1
	jmp			ERROR1
		
;***SLAVE ADDRESS***

NEXT1:
	ldi 		MPR,(0<<TWINT) | (1<<TWEN)
	sts	 		TWCR,MPR 
	ldi 		MPR,SLA+W			; Load SLA+W into TWDR Register
	sts	 		TWDR,MPR
	ldi 		MPR,(1<<TWINT) | (1<<TWEN)|(0<<TWSTA)|(0<<TWSTO)
	sts	 		TWCR,MPR 			; Clear TWINT bit in TWCR to start transmission
									; of address
WAIT2: 
	lds 		MPR,TWCR			; Wait for TWINT Flag set. This indicates that
	sbrs 		MPR,TWINT 			; SLA+W has been transmitted, and ACK/NACK has
	rjmp 		WAIT2 				; been received

	lds 		MPR,TWSR 			; Check value of TWI Status Register. If status
	andi		MPR,$F8				; Mask Prescaler Bits
	cpi 		MPR,MT_SLA_ACK 		; different from MT_SLA_ACK, go to ERROR
	breq		NEXT2
	jmp			ERROR2

;***COMMAND REGISTER***

NEXT2:
	ldi 		MPR,(0<<TWINT) | (1<<TWEN)
	sts	 		TWCR,MPR 
	ldi 		MPR,CommandReg 		; Load data (CommandReg Address) into TWDR
									; Register
	sts 		TWDR,MPR
	ldi 		MPR,(1<<TWINT) | (1<<TWEN)|(0<<TWSTA)|(0<<TWSTO)
	sts 		TWCR,MPR 			; Clear TWINT bit in TWCR to start transmission
									; of data
WAIT3: 

	lds 		MPR,TWCR			; Wait for TWINT Flag set. This indicates that
	sbrs 		MPR,TWINT 			; data has been transmitted, and ACK/NACK has
	rjmp 		WAIT3 				; been received

	lds 		MPR,TWSR 			; Check value of TWI Status Register. If status
	andi		MPR,$F8				; Mask Prescaler Bits
	cpi 		MPR,MT_DATA_ACK 	; different from MT_DATA_ACK, go to ERROR
	breq		NEXT3
	jmp			ERROR3
;Send Stop Condition
NEXT3:
	ldi 		mpr,(1<<TWINT)|(1<<TWSTO)|(1<<TWEN)|(0<<TWSTA)
	sts			TWCR,mpr
;Wait for last transmission to end

Check1:
	lds			mpr,TWCR
	andi		mpr,0b00010000		; Check to see that no transmission is going on
	brne		Check1

;Send Repeated Start
NEXT7:
	ldi 		MPR,(1<<TWINT) | (1<<TWSTA) | (1<<TWEN)|(0<<TWSTO)
	sts	 		TWCR,MPR 			; Send REP_START condition
WAIT8:
	lds 		MPR,TWCR			; Wait for TWINT Flag set. This indicates that
	sbrs 		MPR,TWINT 			; the START condition has been transmitted
	rjmp 		WAIT8
	
	lds 		MPR,TWSR 			; Check value of TWI Status Register. If status
									; different from START, go to ERROR
	andi		MPR,$F8				; Mask Prescaler Bits
	cpi 		MPR,START 		
	breq 		NEXT8
	JMP			ERROR8


;***SLAVE ADDRESS+READ***
NEXT8:
	ldi 		MPR,(0<<TWINT) | (1<<TWEN)
	sts	 		TWCR,MPR
	ldi 		MPR,SLA+R 			; Load SLA+R into TWDR Register

	sts	 		TWDR,MPR
	ldi 		MPR,(1<<TWINT) | (1<<TWEN)| (1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)
	sts	 		TWCR,MPR 			; Clear TWINT bit in TWCR to start transmission
									; of SLA+R
WAIT9:
	lds 		MPR,TWCR			; Wait for TWINT Flag set. This indicates that
	sbrs 		MPR,TWINT 			; SLA+R has been transmitted, and ACK/NACK has
	rjmp 		WAIT9 				; been received

	lds 		MPR,TWSR			; Check value of TWI Status Register. If status
									; different from MR_SLA_ACK, go to ERROR
	andi		MPR,$F8				; Mask Prescaler Bits
	cpi 		MPR,MR_SLA_ACK 		
	brne 		ERROR9


;Get last data Byte

	ldi			MPR,(1<<TWINT) | (1<<TWEN)|(0<<TWSTA)|(0<<TWSTO)
	sts	 		TWCR,MPR 			; Clear TWINT bit in TWCR to start reception of
									; data. Not setting TWEA causes NACK to be
									; returned after reception of next data byte
									; receive last data byte. Signal this to Slave
									; by returning NACK
WAIT11:
	lds 		MPR,TWCR			; Wait for TWINT Flag set. This indicates that
	sbrs 		MPR,TWINT 			; data has been received and NACK returned
	rjmp 		WAIT11

	lds 		MPR,TWSR 			; Check value of TWI Status Register. If status
	andi		MPR,$F8				; Mask Prescaler Bits
	cpi 		MPR,MR_DATA_NACK 	; different from MR_DATA_NACK, go to ERROR
	brne 		ERROR11

	lds			ECHOL,TWDR 			; Input received data from TWDR.

;Issue Stop 

	ldi 		MPR,(1<<TWINT) | (1<<TWSTO) | (1<<TWEN)|(0<<TWSTA)
	sts	 		TWCR,MPR 			; Send STOP signal


mainloop:
	com			ECHOL
	out			PORTC,ECHOL
	rjmp		mainloop

ERROR1:
	ldi			ErrorReg,$01
	rjmp		output
ERROR2:
	ldi			ErrorReg,$02
	rjmp		output
ERROR3:
	ldi			ErrorReg,$03
	rjmp		output
ERROR4:
	ldi			ErrorReg,$04
	rjmp		output
ERROR5:
	ldi			ErrorReg,$05
	rjmp		output
ERROR6:
	ldi			ErrorReg,$06
	rjmp		output
ERROR7:
	ldi			ErrorReg,$07
	rjmp		output
ERROR8:
	ldi			ErrorReg,$08
	RJMP		output
ERROR9:
	ldi			ErrorReg,$09
	RJMP		output
ERROR10:
	ldi			ErrorReg,$0A
	RJMP		output
ERROR11:
	ldi			ErrorReg,$0B
	RJMP		output
Output:

; Load Contents of TWI Status Register and display on Port C (LEDs)

	lds 		mpr2,TWSR			; Load the TWSR for Error display
	or			mpr2,errorreg
	com			mpr2				; Change to active low LEDs
	out			PORTC,MPR2 
LOOP1:
	rjmp		loop1
; *** 65ms delay while Sonar process data
;---------------------------------------------------------------------
Delay65ms:

	push  		XH
	push		XL
	push		mpr2

	ldi			XH,$ff
	ldi			XL,$ff
	ldi			mpr2,$ff

loop:
	sbiw		XH:XL,1
	brne		loop
	dec			mpr2
	brne		loop

	pop 		mpr2

	pop			XL
	pop			XH

	ret

Test:
	ldi		mpr3,$00
	out		portc,mpr3
loop2:
	rjmp	loop2
	ret