View source for 3d stepper source

 ;=======================================================================
 ;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 (low-active) when motors move
 ;     This pin is also connected to a 'busy'-led.
 ; ra.3 serial out
 ; rb.0 serial in
 ; 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 	350     ; 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)
 KeyboardRows	EQU	ra ; port A serves keyboard rows (outputs, a.0-a.2)
 KeyboardRow0	EQU	ra.0
 KeyboardRow1	EQU	ra.1
 KeyboardRow2	EQU	ra.2
 KeyboardColumns EQU	rb ; port B serves keyboard columns (inputs b.1-b.3)
 KeyboardCol0	EQU	rb.1
 KeyboardCol1	EQU	rb.2
 KeyboardCol2	EQU	rb.3
 KeyboardMask	EQU	%00000111
 
 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
 
 ; aliases for 
 MovedX		EQU StepsToGo
 MovedY		EQU StepsToGo
 MovedZ		EQU StepsToGo
 
 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.
 StepperFlags	DS 1 ;
 StepperKeyMoved EQU StepperFlags.0
 
 ; 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
 
  IFDEF UsingMAX232
 RA_latch	equ	%00001111		;SX18/20/28/48/52 port A latch init
  ELSE
 RA_latch	equ	%00000111		;SX18/20/28/48/52 port A latch init
  ENDIF
 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	%11110001		;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 :DoKeyboard1 ; idle, handle keyboard
 		jmp :DoKeyboard2 ; 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 start 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
 
 :DoKeyboard1
 		Dec16 StepperDelayCount
 		Test16 StepperDelayCount
 		jnz :EndStepper; 2
 
 		MovLit16 StepperDelayCount, StepperDelay ;2
 		clr StepsToGo ; 1
 		clr StepsToGo + 1 ;1
 
 		; activate row 1
 		mov m, #DDIR_W
 		mov !KeyboardRows, #((RA_DDIR & ~KeyboardMask) | (KeyboardMask ^  (1 << . KeyboardRow1)))
 		clrb KeyboardRow1 ; 1
 
 		sb KeyboardCol0 ; 1
 		call :Key10 ; 8
 		sb KeyboardCol1 ;1
 		call :Key11
 		sb KeyboardCol2
 		call :Key12
 		setb KeyboardRow1
 		mov StepperState, #StepperCountKeys
 		jmp :EndStepper
 		
 :DoKeyboard2
 		mov StepperState, #StepperIdleKeys
 		; activate row 2
 		mov m, #DDIR_W
 		mov !KeyboardRows, #((RA_DDIR & ~KeyboardMask) | (KeyboardMask ^ (1 << . KeyboardRow2)))
 		clrb KeyboardRow2		
 		sb KeyboardCol0
 		call :Key20
 		sb KeyboardCol1
 		call :Key21
 		sb KeyboardCol2
 		call :Key22
 		setb KeyboardRow2
 
 
 :EndKeyboard	test StepsToGo
 		jnz :DoOutputStepper
 				
 
 		; set all stepper outputs to zero
 		; this should decrease current when steppers are
 		; idle.
 :ZeroStepperOutputs
 		setb KeyboardRow0
 		clr OutputXY
 		mov w, OutputZ
 		and w, #%00001111
 		mov OutputZ, w
 		jmp :EndStepper
 
 :Key10		dec PosX
 		setb StepsToGo.0
 		retp
 :Key11		dec PosY
 		setb StepsToGo.0
 		retp
 :Key12		inc PosX
 		setb StepsToGo.0
 		retp
 :Key20		inc PosY
 		setb StepsToGo.0
 		retp
 :Key21		dec PosZ
 		setb StepsToGo.0
 		retp
 :Key22		inc PosZ
 		setb StepsToGo.0
 		retp
 
 ; 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
 
 
 
 
 :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, %0100, %0010, %1000, %0001, %0100, %0010, %1000
 		retw %0001, %0011, %0010, %0110, %0100, %1100, %1000, %1001
 ;		retw %0001, %0101, %0100, %0110, %0010, %1010, %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
 
 		ORG $0100
 ; 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 StepperState, #StepperCounting
 :DoOutputStepper
 		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
 
 		Test16 StepsToGo
 		jnz :EndStepper ; if steps to go is zero, transition to idle
 		mov StepperState, #StepperIdle
 
 :EndStepper		
 
 EndInterrupt	mov w, #RetiwValue
 		retiw
 		
 ;------------------------ 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 	m, #PLP_W			;point MODE to write PLP register
 		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 LineDrawingBank
 		mov StepperState, #StepperIdleKeys
 		MovLit16 StepperDelayCount, StepperDelay
 
 		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.
 		clrb KeyboardRow0
 		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                   ;
 
 		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
 
 ;*****************************************************************************************
 ;		String constants
 ;*****************************************************************************************
 		org $500
 StringPage	EQU $
 OpeningMessage	DW '3D Stepper Control v1.00.01', 13
 		DW 'J&J Productions 2006', 13 
 		; notice the missing zero at the above line.
 Prompt		DW '>', 0