SX firmware to control 3 stepper motors

Status so far:

• 3D Bresenham line drawing algorithm has been programmed on-chip.
• text parser has been implemented
• serial port VP has not been integrated yet
• keyboard and switch driver has not been implemented yet.

See the source code below.

;=======================================================================
;TITLE:         stepper.src
;
;PURPOSE:       control up to 3 stepper motors to move in straight lines
;		in space. Take commands from a serial port.
;
;AUTHOR:        Danny Havenith
;	Copyright (c) 2006 Danny Havenith
; Use, modification and distribution is subject to the Boost Software
; License, Version 1.0. (See copy at 
; http://www.boost.org/LICENSE_1_0.txt)
;
; UART VP and associated subroutines:
; Copyright © [01/26/1999] Scenix Semiconductor, Inc. All rights reserved.
;
;REVISIONS:
;
;CONNECTIONS:
; ra.0-2 3 keyboard row outputs
; rb.1-3 3 keyboard column inputs for keyboard matrix
;     ra.0 is the row output for the 'emergency keys' 
;     (min, max, emergency stop) and is always enabled when motors move
;     This pin is also connected to a 'busy'-led.
; ra.3 serial in
; rb.0 serial out
; rb.4-7 Z-axis stepper motor output
; rc.0-3 X-axis stepper motor output
; rc.4-7 Y-axis stepper motor output
;
;
;DETAILS:
; Contains 2 VPs: 1) stepper motor controller 2) serial in/out
;=======================================================================


;-------------------------- DEVICE DIRECTIVES --------------------------

		DEVICE		SX28,OSCHS1,TURBO
		DEVICE		STACKX, OPTIONX

		IRC_CAL		IRC_SLOW

		RESET		Initialize
;------------------------------ CONSTANTS ------------------------------

;------------------------------ --------- ------------------------------
; The following constants are meant to be changed to configure for 
; diverse hardware.

Frequency		EQU	4_000_000 ; clock frequency
BaudRate		EQU	9600 	  ; serial port baudrate
InterruptsPerBit 	EQU	3	; samples per serial bit
HalfStepsPerSecond 	EQU 	20     ; stepper motor half steps/sec

; uncomment the following EQU if using a MAX232, since we need to
; reverse the rs-232 signals while not using a MAX232
;UsingMAX232	EQU 1		; we're using a max232 

;------------------------------ --------- ------------------------------
; some derived constants (derived from the ones above)
; These are not meant to be changed manually.
;
		; clock ticks per interrupt
InterruptPeriod	EQU	Frequency/(InterruptsPerBit * BaudRate) + 1
		; value to put in W to obtain the clock ticks per interrupt
		; formulated in this particular way to get rid of 'Literal 
		; truncated to 8 bits' warning
RetiwValue	EQU 	256-InterruptPeriod 
		; Interrupts per stepper motor step
StepperDelay	EQU	Frequency/(HalfStepsPerSecond*InterruptPeriod)

; number of interrupts to pass between detection of the start bit ("flank") and
; the middle of the first bit, plus one (see code for that 'plus one').
StartDelay	EQU InterruptsPerBit/2 + InterruptsPerBit + 1

		; port definitions
OutputXY	EQU	rc ; port C outputs the signals for X(0-3) and Y(4-7) 
OutputZ		EQU	rb ; port B outputs the signals for Z on bits(0-3)
KeyboardColumns	EQU	ra ; port A serves keyboard columns (in a.0-a.2)
KeyboardRows    EQU	rb ; port B serves keyboard rows (in b.1-b.3)
SerialOut	EQU	ra.3 ; pins ra.3 and rb.0 are serial output and
SerialIn	EQU	rb.0 ; input respectively.

;------------------------------ VARIABLES ------------------------------
			ORG	$08
; 'global' bank, registers that can be accessed regardles of the current
; bank.
ErrorTreshold	DS 2	; Error treshold (for Bresenhams)
Step1		DS 2	; Step size in the second dimension
Step2		DS 2	; Step size in the third dimension

; temporary register for interrupt routines
InterruptScratch DS 1
; this byte contains the sign bits for x (.0),y (.1) and z (.2)
Signs		DS 1

			ORG $10
; bank with registers for the line drawing algorithm
LineDrawingBank	EQU $

StepsToGo	DS 2 ; steps left on current line
Error2		DS 2 ; cumulative errr in the first dimension
Error1		DS 2	; cumulative error in the second dimension
StepperDelayCount DS 2  ; timeout between steps
PosX		DS 1 ; X, Y and Z pos. 8 bits precision is more
PosY		DS 1 ; than enough.
PosZ		DS 1

Direction0	DS 1 ; mapping of dimensions (0, 1, 2) to 
Direction1	DS 1 ; axes (x, y, z) and direction (up or down).
Direction2	DS 1

StepperState	DS 1 ; current stepper state. see below.

; interruptable states, these are pre-empted by a move command
StepperIdle	 EQU 0 ; Do Nothing
StepperIdleKeys	 EQU 1 ; Handle keyboard
StepperCountKeys EQU 2 ; Timeout and handle keys

; non-interruptable states. No new vector can be programmed
; while the driver is in one of these states
StepperUpdatePos1 EQU 4 ; Update position 1
StepperUpdatePos2 EQU 5; Update position 2
StepperOutputPos EQU 6 ; latch x,y,z position to outputs
StepperCounting	 EQU 7 ; wait a while before moving to next position
			; update position 0 if wait ends


			ORG $30
		; this bank contains the results of the commands
		; more specifically, the x,y and z vector values
		; are kept here...
CommandBank	EQU $

Coordinates	EQU $
		;coordinates must start at an even address, see the parse function
GoX		DS 2
GoY		DS 2
GoZ		DS 2

		; command parser variables
CurrentNumber	DS 2 ; accumulator for the number we're currently parsing
ParserState	DS 1 ; current state
NextChar	DS 1 ; next character to parse
CurrentCoordinate DS 1; which coordinate we're parsing.
CurrentCommand	DS 1;
CommandFlags	DS 1;


; parser states
ParserExpectCommand	EQU 0
ParserExpectNumber	EQU 1
ParserInHexNumber	EQU 2
ParserExpectReturn	EQU 3


			ORG $50
SERIAL		=       $	;UART bank
tx_high		ds      1	;hi byte to transmit
tx_low		ds      1	;low byte to transmit
tx_count	ds      1	;number of bits sent
tx_divide	ds      1	;xmit timing (/16) counter
rx_count	ds      1	;number of bits received
rx_divide	ds      1	;receive timing counter
rx_byte		ds      1	;buffer for incoming byte
flags		ds	1	;only contains the rx_flag
rx_flag		EQU flags.0
string		ds	1	;used by send_string to store the address in memory
byte		ds	1	;used by serial routines


;---------------------------- SETTINGS ---------------------------

		FREQ	Frequency
	
WKED_W		equ	$0A		;Write MIWU/RB Interrupt edge setup, 0 = falling, 1 = rising
WKEN_W		equ	$0B		;Write MIWU/RB Interrupt edge setup, 0 = enabled, 1 = disabled
ST_W		equ	$0C		;Write Port Schmitt Trigger setup, 0 = enabled, 1 = disabled
LVL_W		equ	$0D		;Write Port Schmitt Trigger setup, 0 = enabled, 1 = disabled
PLP_W		equ	$0E		;Write Port Schmitt Trigger setup, 0 = enabled, 1 = disabled
DDIR_W		equ	$0F		;Write Port Direction

RA_latch	equ	%00000000		;SX18/20/28/48/52 port A latch init
RA_DDIR		equ	%11110000		;see under pin definitions for port A DDIR value
RA_LVL		equ	%00000000		;SX18/20/28/48/52 port A LVL value
RA_PLP		equ	%11111111		;SX18/20/28/48/52 port A PLP value

RB_latch	equ	%00000000		;SX18/20/28/48/52 port B latch init
RB_DDIR		equ	%00001111		;SX18/20/28/48/52 port B DDIR value
RB_ST		equ	%11111111		;SX18/20/28/48/52 port B ST value
RB_LVL		equ	%00000000		;SX18/20/28/48/52 port B LVL value
RB_PLP		equ	%11111111		;SX18/20/28/48/52 port B PLP value

RC_latch	equ	%00000000		;SX18/20/28/48/52 port C latch init
RC_DDIR		equ	%00000000		;SX18/20/28/48/52 port C DDIR value
RC_ST		equ	%11111111		;SX18/20/28/48/52 port C ST value
RC_LVL		equ	%00000000		;SX18/20/28/48/52 port C LVL value
RC_PLP		equ	%11111111		;SX18/20/28/48/52 port C PLP value

;-------------------------- MACRO DEFINITIONS --------------------------
; The following are mainly some macros that define 16 bit, little endian
; versions of the 8-bit instruction set that we all know and have grown to
; love.

; 16-bit little endian decrement
Dec16		MACRO address
		 sub address, #1 ; remember dec doesn't change C flag
		 sc
		 dec address + 1
		ENDM

; 16-bit little endian addition
; This addition sets carry flag accordingly (but not the zero flag)
Add16		MACRO dest, src	; little endian 16 bit addition
		mov	W, src
		add	dest, W
		mov	W, src + 1
		snb	C
		movsz	W, ++src + 1
		add	dest + 1, W
		ENDM

; 16-bit little endian subtraction
Sub16		MACRO dest,src
		sub dest, src	; sub lsb
		subb dest + 1, /c   ; sub one extra if carry not set (= borrow)
		sub dest + 1, src + 1; subtract w from msb
		ENDM
		

; 16-bit, little endian compare, sets zero and carry flag
Cmp16		MACRO dest, src
		LOCAL End
		mov w, src + 1
		mov w, dest + 1 - w
				
		jnz	End   	; if msb equal...
		mov w, src	;...compare lsb
		mov w, dest - w ; 
End
		ENDM
		
; It's 16 bit
; It's Little endian (or it doesn't matter)
; And, it copies.
Copy16		MACRO dest, src
		mov dest, src
		mov dest + 1, src + 1
		ENDM

; test a 16 bit value for zero, start with the msb, to safely test
; when another 'thread' (interrupt) is decreasing the value.
Test16		MACRO dest
		test dest + 1
		snz
		test dest
		ENDM

Clr16		MACRO dest
		clr dest
		clr dest + 1
		ENDM
		
; move a 16 bit literal into RAM
MovLit16	MACRO dest, literal16
		mov dest, #(literal16 // 256)
		mov dest + 1, #(literal16 / 256) 
		ENDM
	
; These macros control whether our serial line is high-active
; or low-active.
Rs232Up		MACRO pin
			IFDEF UsingMAX232 THEN
				setb pin
			ELSE
				clrb pin
			ENDIF
		ENDM

Rs232Down	MACRO pin
			IFDEF UsingMAX232 THEN
				clrb pin
			ELSE
				setb pin
			ENDIF
		ENDM

; transfer the state of the rs232 input pin to the carry flag
Rs232PinToCarry MACRO pin
		sb	pin			;get current rx bit
		IFDEF UsingMAX232 THEN
	        	clc
		ELSE
			stc
		ENDIF
	        snb	pin			; 
		IFDEF UsingMAX232 THEN
			stc
		ELSE
	        	clc		
		ENDIF
		ENDM

; rather complex macro to generate code that 
; produces a line. It puts the absolute components
; in the GoX, GoY and GoZ registers and then composes
; the last three bits of the 'signs' register.
; the expression '||x' takes the absolute value of x.
DoLine		MACRO x,y,z
		bank CommandBank
		MovLit16 GoX, ||x
		MovLit16 GoY, ||y
		MovLit16 GoZ, ||z

		; the following uses the fact that the expression:
		; 	||(a+1) - ||a + 1
		; is zero for a < 0
		; is two otherwise. (for integer a)
		mov signs, # ( (||(x+1) - ||x + 1) / 2 + (||(y+1) - ||y + 1) + (||(z+1) - ||z + 1) * 2)
		call @Move	
		ENDM

; convert from ascii-coded hex digit to value
; jump if not a	true hex digit
; note: this code changes the contents of the fr register
; note: this could probably be optimized
ConvertHexOrJump MACRO fr,address
		LOCAL is_digit
		cjb fr, #'0', address
		cja fr, #'z', address
		cjbe fr, #'9', is_digit
		or fr, #$20
		cjb fr, #'a', address
		cja fr, #'f', address
		sub fr, #('a' - 10 - '0')
is_digit	sub fr, #'0'
		ENDM
		
;-------------------------- INTERRUPT ROUTINE --------------------------
		ORG	0

Interrupt
	;*********************************************************************************
	; Virtual Peripheral: Universal Asynchronous Receiver Transmitter (UART) 
	; These routines send and receive RS232 serial data, and are currently
	; configured (though modifications can be made) for the popular
	; "No parity-checking, 8 data bit, 1 stop bit" (N,8,1) data format.
	; RECEIVING: The rx_flag is set high whenever a valid byte of data has been
	; received and it is the calling routine's responsibility to reset this flag
	; once the incoming data has been collected.
	; TRANSMITTING: The transmit routine requires the data to be inverted
	; and loaded (tx_high+tx_low) register pair (with the inverted 8 data bits
	; stored in tx_high and tx_low bit 7 set high to act as a start bit). Then
	; the number of bits ready for transmission (10=1 start + 8 data + 1 stop)
	; must be loaded into the tx_count register. As soon as this latter is done,
	; the transmit routine immediately begins sending the data.
	; This routine has a varying execution rate and therefore should always be
	; placed after any timing-critical virtual peripherals such as timers,
	; adcs, pwms, etc.
	; Note: The transmit and receive routines are independent and either may be
	;	removed, if not needed, to reduce execution time and memory usage,
	;	as long as the initial "BANK serial" (common) instruction is kept.
	;
	;       Input variable(s) : tx_low (only high bit used), tx_high, tx_count
	;       Output variable(s) : rx_flag, rx_byte
	;       Variable(s) affected : tx_divide, rx_divide, rx_count
	;       Flag(s) affected : rx_flag
	;
	;*********************************************************************************
SerialVP
		bank	SERIAL			;switch to serial register bank
:transmit	decsz	tx_divide		;only execute the transmit routine
		jmp	:receive		; 
		mov	w,#InterruptsPerBit	;
		mov	tx_divide,w		; 
		test    tx_count		;are we sending?
		snz				; 
		jmp	:receive		;
		stc                             ;yes, ready stop bit
		rr      tx_high			; and shift to next bit
		rr      tx_low			; 
		dec     tx_count		;decrement bit counter
		snb	tx_low.6		;output next bit
		Rs232Up	SerialOut		; 
		sb	tx_low.6		; 
		Rs232Down SerialOut		; 
		
		; transfer input to carry flag.
:receive        Rs232PinToCarry SerialIn

		test    rx_count                ;currently receiving byte?
		sz				; 
		jmp	:rxbit                  ;if so, jump ahead
		mov     w,#9                    ;in case start, ready 9 bits
		sc				;skip ahead if not start bit
		mov     rx_count,w              ;it is, so renew bit count
		mov     w,#StartDelay		;ready 1.5 bit periods plus one
		mov     rx_divide,w		; 
:rxbit          decsz	rx_divide		;middle of next bit?
          	jmp	:rxdone			;
		mov	w,#InterruptsPerBit	;yes, ready 1 bit period
		mov	rx_divide,w		; 

		dec     rx_count                ;last bit?
		sz                              ;if not
		rr      rx_byte                 ; then save bit
		snz                             ;if so,
		setb    rx_flag                 ; then set flag
:rxdone						; else, exit

; Stepper motor controller.
; This VP controls up to 3 stepper motors. It implements Bresenhams algorithm
; to move in a straight line in 3D coordinate space.
Stepper	
		bank LineDrawingBank 
		mov w, StepperState
		jmp pc + w
		jmp :ZeroStepperOutputs ; idle, do noting
		jmp :EndStepper ; idle, handle keyboard
		jmp :EndStepper ; timeout while handling keyboard
		jmp :EndStepper ; state 3 not used

; non-interruptable states

		jmp :DoUpdatePos1 ; update pos1
		jmp :DoUpdatePos2 ; update pos2
		jmp :OutputStepper; latch x,y,z-positions to outputs

		; state #7 no jump, just sstart executing here 
		; wait a while before moving to next position
		Dec16 StepperDelayCount
		Test16 StepperDelayCount
		jnz :EndStepper

		; transition to update position state
		mov StepperState, #StepperUpdatePos1
		MovLit16 StepperDelayCount, StepperDelay

		; make a step in the first direction
		Dec16 StepsToGo
		mov w, Direction0
		call MakeStep
		jmp :EndStepper

		; set all stepper outputs to zero
		; this should decrease current when steppers are
		; idle.
:ZeroStepperOutputs
		clr OutputXY
		mov w, OutputZ
		and w, #%00001111
		mov OutputZ, w
		jmp :EndStepper


; update position 1 state		
:DoUpdatePos1
		Add16 Error1, Step1
		jc :Change1
		Cmp16 Error1, ErrorTreshold
		jnc :EndUpdatePos1
		
:Change1		
		Sub16 Error1, ErrorTreshold
		mov w, Direction1
		call MakeStep	
:EndUpdatePos1
		mov StepperState, #StepperUpdatePos2
		jmp :EndStepper


; update position 2 state
:DoUpdatePos2
		Add16 Error2, Step2
		jc :Change2
		Cmp16 Error2, ErrorTreshold
		jnc :TransitionToOutputPos
		
:Change2
		Sub16 Error2, ErrorTreshold
		mov w, Direction2
		call MakeStep

:TransitionToOutputPos
		; transition to outputpos state
		mov StepperState, #StepperOutputPos
		jmp :EndStepper


; output positions state		
:OutputStepper	; now translate x,y and z to stepper
		; line outputs

		; first move x and y to combined 
		; XY output (4 bits each)
		mov w, PosX
		call ExpandPos
		mov InterruptScratch, w
		mov w, PosY
		call ExpandPos2
		or w, InterruptScratch
		mov OutputXY, w
		mov w, PosZ
		call ExpandPos2 ; translate z pos to high bits
		mov InterruptScratch, w
		mov w, OutputZ
		and w, #%00001111
		or w, InterruptScratch
		mov OutputZ, w
		mov StepperState, #StepperCounting

		Test16 StepsToGo
		jnz :EndStepper ; if steps to go is zero, transition to idle
		mov StepperState, #StepperIdle

:EndStepper		

EndInterrupt	mov w, #RetiwValue
		retiw


MakeStep	; increase or decrease an axis dimension, depending on the value of w
		; w.7 determines whether we increase or decrease (1 = increase)
		; w.1 and w.0 determine which axis (0 = x, 1 = y, 2 = z, 3 = crash horibly)
		mov InterruptScratch, w
		jb InterruptScratch.7,	:Increase
		and w, #3
		jmp PC + w
		jmp :DecreaseX
		jmp :DecreaseY
		jmp :DecreaseZ
:Increase	and w, #3
		jmp PC + w
		jmp :IncreaseX
		jmp :IncreaseY
		jmp :IncreaseZ

:IncreaseZ	inc PosZ
		retp
:IncreaseY	inc PosY
		retp
:IncreaseX	inc PosX
		retp
:DecreaseZ	dec PosZ
		retp
:DecreaseY	dec PosY
		retp
:DecreaseX	dec PosX
		retp

ExpandPos	; convert from number (0-3) to 
		; bit pattern (0001, 0010, 0100, 1000)
		and w, #7
		jmp PC+w
;		retw %0001, %0010, %0100, %1000
		retw %0001, %0011, %0010, %0110, %0100, %1100, %1000, %1001

ExpandPos2	; easiest way to create a bit pattern in upper
		; bits is to have another version of Expand
		; instead of swapping
		and w, #7
		jmp PC+w
;		retw %00010000, %00100000, %01000000, %10000000
		retw %00010000, %00110000, %00100000, %01100000, %01000000, %11000000, %10000000, %10010000

		
;------------------------ INITIALIZATION ROUTINE -----------------------

Initialize
		;<group comment>
		mov 	m, #ST_W			;point MODE to write ST register
		mov     w,#RB_ST            	;Setup RB Schmitt Trigger, 0 = enabled, 1 = disabled
		mov	!rb,w		
		mov     w,#RC_ST            	;Setup RC Schmitt Trigger, 0 = enabled, 1 = disabled
		mov	!rc,w	

		mov 	m, #LVL_W			;point MODE to write LVL register
		mov     w,#RA_LVL            	;Setup RA CMOS or TTL levels, 0 = TTL, 1 = CMOS
		mov	!ra,w		 
		mov     w,#RB_LVL            	;Setup RB CMOS or TTL levels, 0 = TTL, 1 = CMOS
		mov	!rb,w		
		mov     w,#RC_LVL            	;Setup RC CMOS or TTL levels, 0 = TTL, 1 = CMOS
		mov	!rc,w	

		mov 	w,#RA_PLP            	;Setup RA Weak Pull-up, 0 = enabled, 1 = disabled
		mov	!ra,w		 
		mov     w,#RB_PLP            	;Setup RB Weak Pull-up, 0 = enabled, 1 = disabled
		mov	!rb,w		
		mov     w,#RC_PLP            	;Setup RC Weak Pull-up, 0 = enabled, 1 = disabled
		mov	!rc,w	

		mov 	m, #DDIR_W			;point MODE to write DDIR register
		mov	w,#RA_DDIR		;Setup RA Direction register, 0 = output, 1 = input		
		mov	!ra,w	
		mov	w,#RB_DDIR		;Setup RB Direction register, 0 = output, 1 = input
		mov	!rb,w			
		mov	w,#RC_DDIR		;Setup RC Direction register, 0 = output, 1 = input
		mov	!rc,w			

		mov     w,#RA_latch          	;Initialize RA data latch
		mov     ra,w		
		mov     w,#RB_latch         	;Initialize RB data latch
		mov     rb,w		
		mov     w,#RC_latch          	;Initialize RC data latch
		mov     rc,w		

; zero all ram (SX28)
		clr	fsr			;reset all ram banks
:zero_ram	sb	fsr.4			;are we on low half of bank?
		setb	fsr.3			;If so, don't touch regs 0-7
		clr	ind			;clear using indirect addressing
		incsz	fsr			;repeat until done
		jmp	:zero_ram

		mov	!option,#%10011111	;enable rtcc interrupt

;---------------------------- MAIN PROGRAM -----------------------------

Main		
		bank SERIAL
		mov w, #OpeningMessage // 256; modulo to prevent warning
		call @send_string
		
		
MainLoop	
		bank SERIAL
		call @get_byte		; read a byte from serial port
		call @send_byte		; echo that same byte
		mov w, byte		; re-retreive the received byte
		bank CommandBank
		mov NextChar, w
		call @ParseCharacter	; parse the byte and act
		test CommandFlags
		snz
		jmp MainLoop
		
		clr CommandFlags
		; for now, we only have 1 command, so assume
		; 'M' was issued.
		call @Move
		mov w, #Prompt // 256
		call @send_string
		jmp MainLoop
		
		ORG $200
SendPrompt	bank SERIAL
		mov w, #'>'
		call @send_byte
		mov w, #13
		call @send_byte
		mov w, #10
		call @send_byte
		retp

Move		; program the vector in GoX, GoY and GoZ
		; into the Bresenham registers and start the
		; algorithm.
		; If the Bresenham routine is busy, wait until it's 
		; finished
		
		bank LineDrawingBank	
		; wait until the stepper is interruptible	
:Wait		jb StepperState.2, :Wait	

		; now start to determine which value is the largest
		; Bresenhams algoritm is driven by the largest component
		; on every 'tick' a step is made in that direction and other
		; directions may or may not make a step in that same tick.
		bank CommandBank
		Cmp16 GoX, GoY
		jnc :YOrZIsLargest ; jump if (Y > X)
:XOrZIsLargest	
		Cmp16 GoX, GoZ	
		jnc :ZIsLargest	; jump if (Z > X)

:XIsLargest	; x is largest, order becomes x, y, z
		Copy16 ErrorTreshold, GoX
		Copy16 Step1, GoY 
		Copy16 Step2, GoZ

		bank LineDrawingBank

		; first set the direction registers
		; the direction registers map Bresenhams registers 
		; (0, 1, and 2) to the three axis (x, y and z)
		; 
		; these are coded 0 for x, 1 for y and 2 for z
		; we set them to a value 'shifted left' (times 2) 
		; because we're going to shift them to the right 
		; lateron to add the direction bits.
		mov Direction0, #0 ; 2 times 0, for x
		mov Direction1, #2 ; 2 times 1, for y
		mov Direction2, #4 ; 2 times 2, for z

		; now distribute the direction (up or down) bits
		rr Signs
		rr Direction0	; x to Direction0
		rr Signs
		rr Direction1	; y to Direction1
		rr Signs
		rr direction2   ; z to Direction2

		jmp :Finalize

:YOrZIsLargest	
		Cmp16 GoY, GoZ
		jnc :ZIsLargest
	
:YIsLargest	; y is largest, order becomes y, x, z
		Copy16 ErrorTreshold, GoY
		Copy16 Step1, GoX
		Copy16 Step2, GoZ
		bank LineDrawingBank

		; first set the direction registers
		; the direction registers map Bresenhams registers 
		; (0, 1, and 2) to the three axis (x, y and z)
		; 
		; these are coded 0 for x, 1 for y and 2 for z
		; we set them to a value 'shifted left' (times 2) 
		; because we're going to shift them to the right 
		; lateron to add the direction bits.
		mov Direction0, #2 ; 2 times 1, for y
		mov Direction1, #0 ; 2 times 0, for x
		mov Direction2, #4 ; 2 times 2, for z

		; now distribute the direction (up or down) bits
		rr Signs
		rr Direction1	; x to Direction1
		rr Signs
		rr Direction0	; y to Direction0
		rr Signs
		rr direction2   ; z to Direction2

		jmp :Finalize
:ZIsLargest	; z is largest, order becomes z, x, y
		Copy16 ErrorTreshold, GoZ
		Copy16 Step1, GoX
		Copy16 Step2, GoY
		bank LineDrawingBank

		; first set the direction registers
		; the direction registers map Bresenhams registers 
		; (0, 1, and 2) to the three axis (x, y and z)
		; 
		; these are coded 0 for x, 1 for y and 2 for z
		; we set them to a value 'shifted left' (times 2) 
		; because we're going to shift them to the right 
		; lateron to add the direction bits.
		mov Direction0, #4 ; 2 times 2, for z
		mov Direction1, #0 ; 2 times 0, for x
		mov Direction2, #2 ; 2 times 1, for y

		; now distribute the direction (up or down) bits
		rr Signs
		rr Direction1	; x to Direction1
		rr Signs
		rr Direction2	; y to Direction2
		rr Signs
		rr direction0   ; z to Direction0

		BREAK :Finalize
:Finalize
		; Error1 = ErrorTreshold/2
		Copy16 Error1, ErrorTreshold
		rr Error1 + 1 
		rr Error1
		
		; Error2 = Error1
		Copy16 Error2, Error1 

		; Set StepsToGo
		Copy16 StepsToGo, ErrorTreshold
		MovLit16 StepperDelayCount, StepperDelay

		; now set the state to 'output position'
		; this will activate the stepper.
		mov StepperState, #StepperOutputPos
		retp

; parse an input character
; This routine is used to parse input command strings
; an example input string would be
;  'm 1ff, 0, FFFF' 
; which stands for 'Move 511, 0, 65535'
ParseCharacter	

		bank CommandBank
		mov w, ParserState
		jmp PC + w
		jmp :DoExpectCommand						
		jmp :DoExpectNumber
		jmp :DoInNumber
:DoExpectReturn
		mov w, #13 ; carriage return
		mov w, NextChar - w
		sz ; do nothing unless we have a cr
		retp
		mov ParserState, #ParserExpectCommand
		or CommandFlags, #$01
		retp ; call the 'move' routine and return
:DoExpectCommand
		mov w, #'M'
		mov w, NextChar - w
		and w, #~$20 ; tolower
		sz ; return if it is not 'm' (the only command we support so far
		retp
		mov CurrentCommand, #'M'
		Clr16 GoX
		Clr16 GoY
		Clr16 GoZ
		mov Signs, #$07; all positive
		mov CurrentCoordinate, #1 ; start parsing x
		mov ParserState, #ParserExpectNumber
		retp
:DoExpectNumber
		ConvertHexOrJump NextChar, :EndExpectNumber
		mov ParserState, #ParserInHexNumber
		mov CurrentNumber, NextChar
		clr CurrentNumber + 1
:EndExpectNumber 
		cse NextChar, #'-' ; return if not '-'...
		retp
		mov w, /CurrentCoordinate
		and Signs, w ; reset the corresponding sign bit
		retp		

:DoInNumber	ConvertHexOrJump NextChar, :EndInNumber
		; now CurrentCoordinate = CurrentCoordinate * 16 + nextdigit
		; todo: optimize
		clc
		rl CurrentNumber
		rl CurrentNumber + 1
		rl CurrentNumber
		rl CurrentNumber + 1
		rl CurrentNumber
		rl CurrentNumber + 1
		rl CurrentNumber
		rl CurrentNumber + 1
		mov w, NextChar
		and w, #$0f
		or CurrentNumber, w
		retp

:EndInNumber	
		; calculate the destination address
		mov w, #Coordinates
		add w, CurrentCoordinate

		and w, #$fe
	
		; copy the parsed number to the address
		mov FSR, w
		mov IND, CurrentNumber
		inc FSR
		mov IND, CurrentNumber + 1

		; if this was not the last coordinate, 
		; goto ExpectNumber state
		; else goto ExpectReturn state
		mov w, #ParserExpectNumber
		snb CurrentCoordinate.2
		mov w, #ParserExpectReturn
		mov ParserState, w

		clc
		rl CurrentCoordinate
		
		; immediately reparse this character
		jmp ParseCharacter
		
		
		
				
		ORG	$400
;*****************************************************************************************
; UART Subroutines
;*****************************************************************************************

	;*********************************************************************************
	; Function: get_byte
	; Get byte via serial port and echo it back to the serial port
	; INPUTS:
	;	-NONE
	; OUTPUTS:
	;	-received byte in rx_byte
	;*********************************************************************************
get_byte	bank 	SERIAL
		sb	rx_flag			;wait till byte is received
		jmp	get_byte
		clrb    rx_flag			;reset the receive flag
		mov     w,rx_byte		;store byte (copy using W)
		mov     byte,w			; & fall through to echo char back
		retp

	;*********************************************************************************
	; Function: send_byte
	; Send byte via serial port
	; INPUTS:
	;	w 	-	The byte to be sent via RS-232
	; OUTPUTS:
	; 	outputs the byte via RS-232
	;*********************************************************************************
send_byte    	bank    SERIAL

:wait        	test    tx_count                ;wait for not busy
		sz
		jmp	:wait                   ;

		;not     w                       ;ready bits (inverse logic)
		mov     tx_high,w               ; store data byte
		clrb    tx_low.7                ; set up start bit
		mov     w,#10			;1 start + 8 data + 1 stop bit
		mov     tx_count,w
		retp                            ;leave and fix page bits

	;*********************************************************************************
	; Function: send_string
	; Send string pointed to by address in W register
	; INPUTS:
	;	w	-	The address of a null-terminated string in program
	;			memory
	; OUTPUTS:
	; 	outputs the string via RS-232
	;*********************************************************************************
send_string	bank	SERIAL
 		mov     string,w                ;store string address
:loop        	mov     w,string                ;read next string character
		mov     m,#(StringPage>>8)      ;with indirect addressing 
		iread                           ;using the mode register
		test    w                       ;are we at the last char?
		snz                             ;if not=0, skip ahead
		jmp	:exit			;yes, leave & fix page bits
		call    send_byte               ;not 0, so send character
		inc     string                  ;point to next character
		jmp     :loop                   ;loop until done
:exit		mov     m,#$0f                   ;reset the mode register
		retp

		org $500
StringPage	EQU $
OpeningMessage	DW '3D Stepper Control v1.0', 13
		DW 'J&J Productions 2006', 13 

Prompt		DW ">", 0