;*********************************************************************************
;* Program:		DROLathe.asm
;*
;* Description:		Program with Tiny2313 AVR RISC Processor,
;*			2x16 LCD display using 4-wire interface .
;* 
;* 	Readout of an incremental encoder.  State processing
;* 	started with timer interrupt.  Resolution = 1 um
;* 
;* 
;* 
;* First version:	Jan 2007
.
;* Target:		Tiny2313
;* Author:		Michael Hompus
;* 			
;* 
;* LCD rs pin connected to PC0  
;* LCD r/w pin connected to Ground
;* LCD e pin connected to PC1			
;* D4-D7 FROM LCD IS CONNECTED TO PORT PB4-PB7 

; Connections on the Tiny2313
; Pin		Function (Connection)
; VCC		+5V (decoupled)
; GND		ground
; XTAL1		Clock (see data sheet)
; XTAL2		Clock (see data sheet)
; TXD PD0		Serial out (RS-232 inverting buffer to remote device - NOT USED)
; RXD PD1		Serial in  (RS-232 inverting receive buffer -NOT USED)
; PD2-PD5	Z and R encoder 
;  
; PB2		R/S pin on  LCD (module pin 4)
; PB3		OE pin on  LCD (module pin 6)
; PB4-PB7	D4-D7 on  LCD (module pins 11-14 respectively)

; PD0		KeyX in
; PD1		KeyR in
;*
;* 
;*
;***************************************************************************
.nolist
.include "tn2313def.inc"	;Define chip particulars
.set xtal=1024
.list
.listmac 
;.set sim=1
;.set WrEprom=1		;write EEprom data
.ifdef sim 
.MESSAGE "Simulation  mode, let op!"
.endif
.ifndef sim
.MESSAGE "Geen simulatie, Downloadable file"
.message __TIME__
.endif

;***** Global register variables
.def	rBin1H	=R12
.def	rBin1L	=R13
.def	rBin2H	=R14
.def	rBin2L	=R15


.def	rmp	=R16	;General use working register
.def	timeout	=R17	;Timeout value passed to subroutine
.def	temp 	=R18
.def	axis	=R19
.def	wreg	=R20
		
.def	time	=R21
.def	Spare	=R22
.def	Mykey	=R23
.def	pos	=R24

;***** Other equates
.equ	lcdrs	=PB2		;LCD rs pin 
;.equ	lcdrw	=gnd		;LCD r/w pin 
.equ	lcde	=PB3		;LCD e pin
.equ    OE	=8	;Bit 3 port B display enable (also directly addressed).
.equ    RS	=4	;Bit 2 in port B display register select (also directly addressed).


.equ	KeyX = 0
.equ	KeyY = 1

.equ	clearlcd=1 		;clear commando
.equ	line1   =0x80		; LCD
.equ	line2   =line1+0x40	; LCD


.equ	X_enc_port=PinD		;port
.equ	X_enc_A=2		;port bit voor A 
.equ	X_enc_B=3		;port bit voor B 

.equ	Y_enc_port=PinD		;port
.equ	Y_enc_A=4		;port bit voor A 
.equ	Y_enc_B=5		;port bit voor B 


.equ	inc_dec=0
.equ	x1_x2=1

;---------------------------------------------------------------------------
;Define the data area for the various channels
;
.DSEG
.org	$60
DataStart:
XData:
XState:	.byte 1
XPosRaw:.byte 2		;2byte	Hibyte first,Lo byte next
XPosRel:.byte 2
XAsc:	.byte 5		;5 Ascii chars
Xend:	.byte 1		;last to signify end of ascii for display routine
XAscmm:	.byte 8		;sign + 6 chars and $0 byte
XMode:	.byte 1		;b0:0=inc/1=dec b1:0=x1/1=x2(for diameter)
YData:
YState:	.byte 1
YPosRaw:.byte 2		;2byte	Hibyte first,Lo byte next
YPosRel:.byte 2
YAsc:	.byte 5		;5 Ascii chars
Yend:	.byte 1		;
YAscmm:	.byte 8
YMode:	.byte 1		;Calibration value for conversion

DataEnd:


.equ	StateOS  = XState-XData		;Offsets of variables
.equ	PosRawOS = XPosRaw-XData
.equ	AscOS    = XAsc-XData
.equ	ModeOS	 = XMode-XAscmm
 
;---------------------------------------------------------------------------
;Define the data area for the values in EEPROM
;
.eseg
XModeE:	.db	0x00	;default= normal, x1
YModeE:	.db	0x02	;default= normal, x2

.org 0x30
	.db "(C) 2007 Michael Hompus"




;***** Macro definitions
.macro	SetZPtr				
	ldi	ZH,high(@0)
	ldi	ZL,low(@0)
.endmacro

.macro	SetSPtr				
	ldi	ZH,high(@0*2)
	ldi	ZL,low(@0*2)
.endmacro

.macro	SetXPtr				
	ldi	XH,high(@0)
	ldi	XL,low(@0)
.endmacro

/*
.macro	SetYPtr				
	ldi	YH,high(@0)
	ldi	YL,low(@0)
.endmacro

.macro	CopyXtoZ				
	mov	ZH, XH
	mov	ZL, XL
.endmacro

.macro	CopyZtoY				
	mov	YH, ZH
	mov	YL, ZL
.endmacro
*/
.macro pushX
	push XL
	push XH
.endmacro

.macro popX
	pop XH
	pop XL
.endmacro

.macro pushY
	push YL
	push YH
.endmacro

.macro popY
	pop YH
	pop YL
.endmacro

.macro pushZ
	push ZL
	push ZH
.endmacro
		
.macro popZ
	pop ZH
	pop ZL
.endmacro
/*
.macro swapYZ
	push	YL
	mov	YL,ZL
	pop	ZL
	push	YH
	mov	YH,ZH
	pop	ZH
.endmacro
*/

;=========================================================================
;	Main program entry point on reset
;=========================================================================
.cseg
.org $0000	rjmp	_reset_

.org OVF0addr
		rjmp	_TimerV0_

_reset_: 
		ldi	temp,LOW(RAMEND) 	;Init Stack Pointer
		out	SPL,temp

;------------------- Init Port B -----------------------------------------

		ldi	temp,0b00000011	;Pullups on inputs B1 and B0
		out	PORTB,temp
		ldi     temp,0b11111100    
		out     DDRB,temp	;all outputs except B1 and B0 

;------------------- Init Port D -----------------------------------------

		ldi	temp,0b01111100	;Pullups on some inputs 
		out	PORTD,temp
		ldi     temp,0b00000001    
		out     DDRD,temp	;PORTd = all inputs

		rcall	InitDisplay

		sbis	pinb, KeyX
		rjmp	Intro
		sbic	pinb, KeyY
		rjmp	ConfigAxes
	
Intro:		rcall	ShowIntro

		rcall	InitRam

		rcall	ShowFrame

;------------------ Init TIMSK,TCCR0,TCNT0 & interrupts -------------------

		ldi TIME, 200 		; Start from 128. NB!
		out TCNT0, TIME 	; set Timer/counter also.
		ldi TEMP, (1<<TOIE0) 	; timer overflow interrupt enable 0
		out TIMSK, TEMP
		ldi TEMP, $02 		; clock prescaler = clk/8
		out TCCR0, TEMP
	
		sei			;set global int flag in SREG
;=========================================================================
; main loop
;-------------------------------------------------------------------------
loop:		
;----------- check the keys ----------------------------------------------
		sbic	pinb, KeyX	;check if keys are pressed
		rcall	ClearXabs
		sbic	pinb, KeyY
		rcall	ClearYabs

;----------- process X encoder -----------------------------

		setZptr	XascMM
		lds	R2, XposRaw	;load the position
		lds	R1, XPosRaw+1	; |
		rcall 	Convert		; and convert it to ascii
		ldi	wreg,line1 + 4
		rcall	SendCommand
		rcall	LoadByteS
	
;----------- process Y encoder -----------------------------

		setZptr	YascMM
		lds	R2, YposRaw	;load the position
		lds	R1, YPosRaw+1	; |
		rcall 	Convert		; and convert it to ascii
		ldi	wreg,line2 + 4
		rcall	SendCommand
		rcall	LoadByteS

		ldi 	timeout,255
		rcall 	delay		;timeout x 20 usec

		rjmp loop

;=========================================================================
;		Routines for zeroing
;=========================================================================

ClearXabs:	setZptr	XPosRaw
		rjmp ClearPos

ClearYabs:	setZptr	YPosRaw
		
ClearPos:	clr	R0
		st	Z+, R0
		st	Z,  R0
		ret

;=========================================================================
;		Display routines
;=========================================================================
.include "2x16lcd.inc"	

;=========================================================================
;		Delay loops
;=========================================================================
longdelay:

.ifdef sim
		ret		; to speed up simulation
.endif
		ldi	wreg,50	
		ldi	timeout,50
puwait:
		rcall	delay
		dec	wreg
		brne	puwait
		ret
;------------- Delay timeout*20 microseconds ------------------------------
delay:		
.ifdef sim
		ret
.endif	
		ldi 	temp,51 ;1 cycle 
cagain:		
		dec	temp	;1 cycle
		nop		;1 cycle
		nop		;1 cycle
		brne 	cagain	;2 cycles	

		dec 	timeout	;1 cycle
		brne 	delay	;2 cycles
		ret

;=========================================================================
;	Send a string from Program memory to LCD
;=========================================================================
;load byte from Program memory
LoadByteP:
		lpm			; Load byte from program memory into r0
		tst	r0		; Check for the end of the message
		breq	lbp1		;  If so, quit
		mov	wreg,r0
		rcall	SendData	; to display
		adiw	ZL,1		; Increase Z registers
		rjmp	LoadByteP
lbp1:		ret
;=========================================================================
;	Send a string from SRAM memory to LCD
;=========================================================================
LoadByteS:
		ld	wreg,Z+		; Load byte from sram memory into wreg
lbs1:		tst	wreg		; Check for end of the message
		breq	lbs2		;  If so, quit
		rcall	SendData	; char to display
		ld	wreg,Z+		; get next char
		rjmp	lbs1
lbs2:		ret
;=========================================================================
;	Display some initial texts
;=========================================================================
ShowIntro:
		rcall	Homeone
		SetSPtr	introL1		;Type the messageto LCD
		rcall	LoadByteP
	
		ldi	timeout,200	
		rcall	delay

		rcall	Hometwo
		SetSPtr	introL2
		rcall	LoadByteP	;Type the messageto LCD

		rcall	longdelay
		rcall	longdelay
		rcall	longdelay
	
		ldi	wreg,clearlcd		;Clear the screen		
		rcall	SendCommand
		ldi	timeout,200		;Delay 15 mS for clear command
		rcall	delay
		ret
;-----------------------------------------------------------------------------
;write the coordinate layout frame

ShowFrame:	rcall	Homeone
		SetSPtr	PosL1
		rcall	LoadByteP
		
		rcall	Hometwo
		SetSPtr	PosL2
	;	adiw	ZL,2			;alternative: set Z to next msg
		rcall	LoadByteP

		ret
;=========================================================================
;	Message area
;=========================================================================
introL1:
.db	"Digital  Readout",0,0
introL2:
.db	" (c) M. Hompus  ",0,0 

PosL1:
.db	"X               ",0,0
PosL2:
.db	"R               ",0,0


;=========================================================================
;		Interrupt Routines
;=========================================================================
_TimerV0_:
		out TCNT0, TIME ; reset time
	
		pushz
		pushy
		pushx
		push	r16
		in	r16,sreg
		rcall 	ProcessEncoder
		out	sreg,r16
		pop	r16
	   	popx
		popy
		popz
		reti

;==========================================================================
;           Initialise the Ram data area
;==========================================================================
InitRam:	ldi	temp,(DataEnd-DataStart)	;Clear Ram
		ldi	wreg, 0
		setZptr XData
iniclr:		st	z+,wreg
		sbci	temp,1
		brbc	SREG_C,iniclr

;-----------get axis-mode values from EEPROM --------------------------
		ldi	temp, XmodeE	;setup the Eprom address
		rcall	EEPROM_read
		sts	Xmode, rmp	;copy to Ram area
		ldi	temp, YmodeE
		rcall	EEPROM_read
		sts	Ymode, rmp
		ret
	
;==========================================================================
;Eprom routines
;==========================================================================
;Read eprom location pointed to by temp, data returned into rmp
;Write eprom location pointed to by temp, present data in rmp
 

EEPROM_read:	sbic	EECR,EEWE	; Wait for completion of previous read
		rjmp	EEPROM_read
		out	EEAR, temp	; Set up address (r17) in address register
		sbi	EECR,EERE	; Start eeprom read by writing EERE
		in	rmp, EEDR	; Read data from data register
		ret
		
EEPROM_write:	sbic	EECR,EEWE	; Wait for completion of previous write
		rjmp	EEPROM_write	
		out	EEAR, temp	; Set up address (r17) in address register
		out	EEDR, rmp	; Write data (r16) to data register
		sbi	EECR,EEMWE	; Write logical one to EEMWE
		sbi	EECR,EEWE	; Start eeprom write by setting EEWE
		ret		


;=========================================================================
;		State Machine for encoder processing
;=========================================================================
;Left/RightEncoderInterrupt
;
;Called from PWM overflow interrupt. Does quadrature decoding of our
;encoder inputs. 
;
;There are a bunch of invalid state changes (state0 -> state2 for example)
; and this code completely ignores those possibilities. If the rate of
; change is lower than the polling rate of this code, then there can't
; be any invalid state changes.
;
; Registers Used: XL,XH,YL.YH,ZL,ZH
;-
.equ	A = 1<<0 ; Encoder A Line
.equ	B = 1<<1 ; Encoder B line
.equ	ST0 = 0 ; Increment if 0->1, 1->2, 2->3 or 3->0
.equ	ST1 = B ; Decrement if 0->3, 3->2, 2->1 or 1->0
.equ	ST2 = A + B
.equ	ST3 = A
 
ProcessEncoder:

	clr    Xh
	sbic   X_Enc_Port, X_Enc_A
	sbr    Xh, A
	sbic   X_Enc_Port, X_Enc_B
	sbr    Xh, B
	ldi    Zl, low(XData)
	ldi    Zh, high(XData)
	rcall  DecodeState

	clr    Xh
	sbic   Y_Enc_Port, Y_Enc_A
	sbr    Xh, A
	sbic   Y_Enc_Port, Y_Enc_B
	sbr    Xh, B
	ldi    Zl, low(YData)
	ldi    Zh, high(YData)

DecodeState:
	ldd    Xl, Z + StateOS
	ldd    Yl, Z + PosRawOS+1
	ldd    Yh, Z + PosRawOS
	cp     Xh, Xl
	breq   DecodeExit
State0:
	cpi    Xl, ST0
	brne   State1
	cpi    Xh, ST1
	breq   Increment
	cpi    Xh, ST3
	breq   Decrement
	rjmp   DecodeExit
State1:
	cpi    Xl, ST1
	brne   State2
	cpi    Xh, ST2
	breq   Increment
	cpi    Xh, ST0
	breq   Decrement
	rjmp   DecodeExit
State2:
	cpi    Xl, ST2
	brne   State3
	cpi    Xh, ST3
	breq   Increment
	cpi    Xh, ST1
	breq   Decrement
	rjmp   DecodeExit
State3:
    	cpi    Xh, ST0
    	breq   Increment
    	cpi    Xh, ST2
    	brne   DecodeExit
Decrement:
    	sbiw   Yl, 2 	; Save a word and a cycle by falling
Increment:       	; through
    	adiw   Yl, 1
DecodeExit:
    	std    Z + StateOS, Xh
    	std    Z + PosRawOS + 1, Yl
    	std    Z + PosRawOS, Yh
	
    	ret 
;=========================================================================
; Number conversion routines
;=========================================================================
; Bin2ToAsc
; =========
; converts a 16-bit-binary to a 5-digit ASCII coded decimal,
;   the pointer points to the first significant digit of the
;   decimal, returns the number of digits
; In: 	16-bit-binary in rBin1H:L, Z points to first digit of
;   	the ASCII decimal (requires 5 digits buffer space, even
;   	if the number is smaller!)
; Out: Z points to the first significant digit of the ASCII
;   	decimal, rBin2L has the number of characters (1..5)
; Used registers: rBin1H:L (unchanged), rBin2H (changed),
;   			rBin2L (result, length of number), rmp
; Called subroutines: Bin2ToBcd5, Bin2ToAsc5
;
Bin2ToAsc:
	rcall	Bin2ToAsc5 	; Convert binary to ASCII
	ldi 	rmp,6 		; Counter is 6
	mov 	rBin2L,rmp
Bin2ToAsca:
	dec 	rBin2L 		; decrement counter
	ld 	rmp,z+ 		; read char and inc pointer
	cpi 	rmp,' ' 	; was a blank?
	breq 	Bin2ToAsca 	; Yes, was a blank
	sbiw 	ZL,1 		; one char backwards
	ret 
;----------------------------------------------------------------------------
; Bin2ToAsc5
; ==========
; converts a 16-bit-binary to a 5 digit ASCII-coded decimal
; In: 	16-bit-binary in rBin1H:L, Z points to the highest
;   	of 5 ASCII digits, where the result goes to
; Out: 	Z points to the beginning of the ASCII string, 
;	leading zeros are filled with blanks
; Used registers: rBin1H:L (content is not changed),
;   		rBin2H:L (content is changed), rmp
; Called subroutines: Bin2ToBcd5
;
Bin2ToAsc5:
	rcall 	Bin2ToBcd5 	; convert binary to BCD
	ldi 	rmp,4 		; Counter is 4 leading digits
	mov 	rBin2L,rmp
Bin2ToAsc5a:
	ld 	rmp,z 		; read a BCD digit
	tst 	rmp 		; check if leading zero
	brne 	Bin2ToAsc5b 	; No, found digit >0
	ldi 	rmp,' ' 	; overwrite with blank
	st 	z+,rmp 		; store and set to next position
	dec 	rBin2L 		; decrement counter
	brne 	Bin2ToAsc5a 	; further leading blanks
	ld 	rmp,z 		; Read the last BCD
Bin2ToAsc5b:
	inc 	rBin2L 		; one more char
Bin2ToAsc5c:
	subi 	rmp,-'0' 	; Add ASCII-0
	st 	z+,rmp 		; store and inc pointer
	ld 	rmp,z 		; read next char
	dec 	rBin2L 		; more chars?
	brne 	Bin2ToAsc5c 	; yes, go on
	sbiw 	ZL,5 		; Pointer to beginning of the BCD
	ret 
;-----------------------------------------------------------------------------
; Bin2ToBcd5
; ==========
; converts a 16-bit-binary to a 5-digit-BCD
; In: 	16-bit-binary in rBin1H:L, Z points to first digit
;   	where the result goes to
; Out: 	5-digit-BCD, Z points to first BCD-digit
; Used registers: rBin1H:L (unchanged), rBin2H:L (changed), rmp
; Called subroutines: Bin2ToDigit
;
Bin2ToBcd5:
	push 	rBin1H 		; Save number
	push 	rBin1L
	ldi 	rmp,HIGH(10000) ; Start with tenthousands
	mov 	rBin2H,rmp
	ldi 	rmp,LOW(10000)
	mov 	rBin2L,rmp
	rcall 	Bin2ToDigit 	; Calculate digit
	ldi 	rmp,HIGH(1000) 	; Next with thousands
	mov 	rBin2H,rmp
	ldi 	rmp,LOW(1000)
	mov 	rBin2L,rmp
	rcall 	Bin2ToDigit 	; Calculate digit
	ldi 	rmp,HIGH(100) 	; Next with hundreds
	mov 	rBin2H,rmp
	ldi 	rmp,LOW(100)
	mov 	rBin2L,rmp
	rcall 	Bin2ToDigit 	; Calculate digit
	ldi 	rmp,HIGH(10) 	; Next with tens
	mov 	rBin2H,rmp
	ldi 	rmp,LOW(10)
	mov 	rBin2L,rmp
	rcall 	Bin2ToDigit 	; Calculate digit
	st 	z,rBin1L 	; Remainder are ones
	sbiw 	ZL,4 		; Put pointer to first BCD
	pop 	rBin1L 		; Restore original binary
	pop 	rBin1H
	ret 
	
;-----------------------------------------------------------------------------
; Bin2ToDigit
; ===========
; converts one decimal digit by continued subraction of a
;   binary coded decimal
; Used by: Bin2ToBcd5, Bin2ToAsc5, Bin2ToAsc
; In: 16-bit-binary in rBin1H:L, binary coded decimal in
;   rBin2H:L, Z points to current BCD digit
; Out: Result in Z, Z incremented
; Used registers: rBin1H:L (holds remainder of the binary),
;   rBin2H:L (unchanged), rmp
; Called subroutines: -
;
Bin2ToDigit:
	clr 	rmp ; digit count is zero
Bin2ToDigita:
	cp 	rBin1H,rBin2H	; Number bigger than decimal?
	brcs 	Bin2ToDigitc	; MSB smaller than decimal
	brne 	Bin2ToDigitb	; MSB bigger than decimal
	cp 	rBin1L,rBin2L	; LSB bigger or equal decimal
	brcs	 Bin2ToDigitc	; LSB smaller than decimal
Bin2ToDigitb:
	sub 	rBin1L,rBin2L	; Subtract LSB decimal
	sbc 	rBin1H,rBin2H	; Subtract MSB decimal
	inc 	rmp		; Increment digit count
	rjmp 	Bin2ToDigita 	; Next loop
Bin2ToDigitc:
	st 	z+,rmp 		; Save digit and increment
	ret 				


;==========================================================================
;Counts naar mm conversie
;=======================================================================
;
; The task: You read in a 10-bit encoder count in the range from 
;    hex 0000 to 3000. You need to convert this into a floating point
;    number in the range from 000.00 to 200.00 mm
;
; The program scheme:
;    1. Check that the number is smaller than $0400.
;       Prevent illegal overflows during the following multiplication.
;    2. Multiplication by <number>
;    3. Round the result and cut the last two bytes of the result.
;       This step divides by 1024 by shifting and ignoring the
;       last byte of the result. 
;    4. Convert the resulting word to ASCII with correct decimal point
;       
;
; The registers used:
;    The routines use the registers R10..R1 without
;    saving these before. Also required is a multi-purpose register 
;    called rmp, located in the upper half of the registers. Please 
;    take care that this register doesn't conflict with the
;    register use in the rest of your program.
;
;    When entering the routine the 10-bit number is expected in 
;    the register pair R2:R1. If the number is greater than $03FF
;    then the check routine returns with the carry flag set, and
;    the resulting string in R5:R6:R7:R8:R9:R10 is set to the 
;    ASCII-string "E.EEEE".
;    The multiplication uses R6:R5:R4:R3 to hold the multiplicator
;    4335  The result of the multiplication is calculated in the
;    registers R10:R9:R8:R7.
;    The result of the so called division by 1024 is copied to R2:R1.
;    Conversion to an ASCII-string uses the input
;    in R2:R1, the register pair R4:R3 as a divisor
;    for conversion, and places the ASCII result in SRAM at a location
;    pointed to by Z.
;
; Other conventions:
;   The conversion uses subroutines and the stack. The stack must 
;   work fine for the use of three levels (six bytes SRAM).
;
; Conversion times:
;   The whole routine requires 80 usec with a 8Mhz clock
;----------------------------------------------------------------------
;
;  On entry:
;   R2:R1 contain the position value to be converted
;   ZH and ZL point to SRamAddress where Ascii is to be written
;   Multiplier is fixed
;  On exit:
;   SRAM(ZH:ZL) contains the Ascii chars representing the original number
;   Including sign

Convert:

	rcall	fpconv10	; call the conversion routine

	ret
;
; Conversion routine wrapper, calls the different conversion steps
;
fpconv10:
	ldd	temp, Z+ModeOS
	sbrc	temp, inc_dec	; check axis direction mode
	rcall 	fpconv10e	; invert rawdata if necessary

	rcall 	fpconv10c 	; Check the input value in R2:R1
	brpl	fpv1
	rcall 	fpconv10e 	; if carry set, complement
fpv1:	;	rcall 	fpconv10m 	; multiply by 1024
	;	rcall 	fpconv10r 	; round and divide by 1024



	
	ldd	temp, Z+ModeOS
	sbrc	temp, x1_x2		; x1 or x2 (radius/diameter)
	rcall	fpconv10x2	; multiply by 2 if necessary

	rcall 	fpconv10a 	; convert to ASCII string
	ret

fpconv10e:		;create 2-s complement 
	com	R2
	com	R1
	inc	R1
	brcs	fpv1e1
	inc	R2
fpv1e1:	
	ret

;
; Subroutine inputcheck
;
fpconv10c:
	clt
	tst 	R2 	;check if negative
	brpl	fpc0c2	; set T bit if negative
	set		; |
fpc0c2:	ret
;
; Subroutine multiplication 
;
; Starting conditions:
; +---+---+
; | R2| R1|  Input number
; +---+---+
; +---+---+---+---+
; | R6| R5| R4| R3| Multiplicant 
; | 00| 0p| qr| st|
; +---+---+---+---+
; +---+---+---+---+
; |R10| R9| R8| R7| Result
; | 00| 00| 00| 00|
; +---+---+---+---+
;
fpconv10m:
	ldi 	rmp, $00 ; set the multiplicant to $0400
	mov 	R6, rmp
	ldi 	rmp, $00
	mov 	R5, rmp
	ldi 	rmp, $04
	mov 	R4, rmp
	ldi 	rmp, $00
	mov 	R3, rmp
	clr 	R10 	; clear the result
	clr 	R9
	clr 	R8
	clr 	R7
fpconv10m1:
	mov 	rmp, R1 	; check if the number is clear
	or 	rmp, R2 	; any bit of the word a one?
	brne 	fpconv10m2 	; still one's, go on convert
	ret 			; ready, return back
fpconv10m2:
	lsr 	R2 		; shift MSB to the right (div by 2)
	ror 	R1 		; rotate LSB to the right and set bit 7
	brcc 	fpconv10m3 	; if the lowest bit was 0, then skip adding
        add 	R7, R3 		; add the number in R6:R5:R4:R3 to the result
	adc 	R8, R4
	adc 	R9, R5
	adc 	R10, R6
fpconv10m3:
	lsl 	R3 		; multiply R6:R5:R4:R3 by 2
	rol 	R4
	rol 	R5
	rol 	R6
	rjmp 	fpconv10m1 	; repeat for next bit
;
; Round the value in R10:R9 with the value in bit 7 of R8
;
fpconv10r:
;	clr 	rmp 	; put zero to rmp
;	lsl 	R8 	; rotate bit 7 to carry
;	adc 	R9,rmp 	; add LSB with carry
;	adc 	R10,rmp ; add MSB with carry
;	mov 	R2,R10 	; copy the value to R2:R1 (divide by 65536)
;	mov 	R1,R9
;	ret
	
	;divide by 1024
	ror	R10	;divide by 2, No ror on R7 since R7 is not needed anyway
	ror	R9	; |
	ror	R8	; |
	ror	R10	;divide by 2 once more
	ror	R9
	ror	R8
	mov 	R2, R9 	; copy the value to R2:R1 (divide by 256)
	mov 	R1, R8
	ret

;
; Convert the word in R2:R1 to an ASCII string in R5:R6:R7:R8:R9:R10
;
; +---+---+
; + R2| R1| Input value 0..6000
; +---+---+
; +---+---+
; | R4| R3| Decimal divider value
; +---+---+
; +---+---+---+---+---+---+
; | R5| R6| R7| R8| R9|R10| Resulting ASCII string 
; |'1'|'9'|'7'|'.'|'5'| 3 | 
; +---+---+---+---+---+---+


fpconv10x2:			;multiply raw data * 2
	rol	R1
	rol	R2
	ret

;
fpconv10a:

	ldi	rmp, '-'
	brts	fpc1a1
	ldi	rmp, '+'
fpc1a1:	std 	Z + 0 , rmp
	ldi 	rmp, HIGH(10000); Set the decimal divider value to 10000
	mov 	R4, rmp
	ldi 	rmp, LOW(10000)
	mov 	R3, rmp
	rcall 	fpconv10d 	; get ASCII digit by repeated subtraction
	std 	Z + 1, rmp 	; set thousands string char

	ldi 	rmp, HIGH(1000)	; Set the decimal divider value to 1000
	mov 	R4, rmp
	ldi 	rmp, LOW(1000)
	mov 	R3, rmp
	rcall 	fpconv10d 	; get ASCII digit by repeated subtraction
	std 	Z + 2, rmp  

	clr 	R4		; Set the decimal divider value to 100
	ldi 	rmp, 100
	mov 	R3, rmp
	rcall 	fpconv10d 	; get ASCII digit by repeated subtraction
	std 	Z + 3, rmp 

	ldi	rmp, '.'	; set te decimal point
	std 	Z + 4, rmp 

	clr 	R4		; Set the decimal divider value to 10
	ldi 	rmp, 10
	mov 	R3, rmp
	rcall 	fpconv10d 	; get ASCII digit by repeated subtraction
	std 	Z + 5, rmp  

	clr 	R4		; Set the decimal divider value to 1
	ldi 	rmp, 1
	mov 	R3, rmp
	rcall 	fpconv10d 	; get ASCII digit by repeated subtraction
	std 	Z + 6, rmp  

	ret

; Convert binary word in R2:R1 to a decimal digit by substracting
; the decimal divider value in R4:R3 (1000, 100, 10)
;
fpconv10d:
	ldi	 rmp,'0' 	; start with decimal value 0
fpconv10d1:
	cp	 R1,R3 		; Compare word with decimal divider value
	cpc	 R2,R4
	brcc	 fpconv10d2 	; Carry clear, subtract divider value
	ret 			; done subtraction

fpconv10d2:
	sub	 R1,R3 		; subtract divider value
	sbc	 R2,R4
	inc 	 rmp 		; up one digit
	rjmp	 fpconv10d1 	; once again
;
; End of floating point conversion routines
;
;=========================================================================
;		Configuration menu routine
;=========================================================================
.macro KeyWait
	nop
.endmacro

.macro Write
	ldi	wreg,@0
	rcall	SendCommand
	SetSPtr	@1
	rcall	LoadByteP
.endmacro

.macro checkit
@1_1:		sbic	pinb, KeyX	;check if keys are released
		rjmp	@1_1
		sbic	pinb, KeyY
		rjmp	@1_1
		rcall	delay2ms

		ldi	Spare, 1<<@0	;get the config bitvalue into Spare
		sbrc	rmp, @0		;check the config bit, and display value		
		rjmp	@1_2
		Write line1+4, @2
		rjmp	@1_3
@1_2:		Write line1+4, @3
@1_3:		sbic	pinb, KeyY	;check the OK-key (=pinB_1)
		rjmp	@1_4
		sbic	pinb, KeyX	;check the toggle-key (=pinB_0)
		eor	rmp, Spare	;invert the config bit if needed
		rjmp	@1_1		; and display and keycheck again
@1_4:		nop			; or continue with next....
.endmacro
;--------------------------------------------------------------------------------------
ConfigAxes:
	;write the conf intro
		rcall	HomeOne
		SetSPtr	ConfMsg
		rcall	LoadByteP
		rcall	HomeTwo
		SetSPtr	Conf2Msg
		rcall	LoadByteP

Cwait:		sbic	pinb, KeyY	;check if  both keys are released
		rjmp	Cwait
		sbic	pinb, KeyX
		rjmp	Cwait


	;write the OK line
		rcall	HomeTwo
		SetSPtr	OKMsg
		rcall	LoadByteP

	;X axis menu
		Write line1, XasMsg
		
		ldi	temp, XmodeE		;get config data from EEPROM
		rcall	EEPROM_read
		
		checkit inc_dec,lblA,  NormMsg, InvMsg	;do the dialog
		checkit x1_x2,  lblB,  x1Msg,   x2Msg

		rcall	EEPROM_write		;write new values back to EEPROM

		Write line1, YasMsg
		ldi	temp, YmodeE
		rcall	EEPROM_read

		checkit inc_dec,lblC, NormMsg, InvMsg
		checkit x1_x2,  lblD, x1Msg,   x2Msg
		rcall	EEPROM_write

		rjmp	Intro			;continue with normal program

		
/*
.macro skip
	sbis @0 ,@1
@1_a:	nop
	ret
.endmacro

	skip pinB, KeyX


.macro jump
	rjmp @0 
.endmacro

	jump ConYx2

*/

;=========================================================================
;		Config Messages
;=========================================================================
ConfMsg:
.db		"Config Menu,druk",0,0
Conf2Msg:
.db		"   een toets    ",0,0
OKMsg:
.db		"              OK",0,0
XasMsg:
.db		"X-as            ",0,0
YasMsg:
.db		"Y-as            ",0,0
NormMsg:
.db		    " normaal   ?",0,0
InvMsg:
.db		    " invers    ?",0,0
x1Msg:
.db		    " Aanw. x1  ?",0,0
x2Msg:
.db		    " Aanw. x2  ?",0,0

.exit
