SX firmware to control 3 stepper motors

Status so far:

• 3D Bresenham 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)
;
;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 ------------------------------
Frequency	EQU	4_000_000 ; clock frequency
BaudRate	EQU	9600 	  ; serial port baudrate
SamplesPerBit	EQU	3	; samples per serial bit
HalfStepsPerSecond EQU 	80     ; stepper motor half steps/sec

; some derived constants (derived from the ones above)
		; clock ticks per interrupt
InterruptPeriod	EQU	Frequency/(SamplesPerBit * 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)

		; 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;

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




;---------------------------- DEBUG SETTINGS ---------------------------

		FREQ	Frequency
	
;		WATCH	<Symbol>,<bit count>,<format>

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	%11110111		;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	%00000001		;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) // 256)
		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

; 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 CommandBank

		mov TextIndex, #TestCommands
:nextchar
		mov m, #TestCommands >> 8
		mov w, TextIndex
		iread
		or w, #0
		jz MainLoop
		mov NextChar, w
		call @ParseCharacter
		bank  CommandBank
		inc TextIndex
		jmp :nextchar

MainLoop	jmp MainLoop
		
		
		
TestCommands
		dw 'm200,200,200', 13
		dw 'M0,-200,0', 13
		dw 'm-200,200,2000', 13
		dw 'm0,-200,0', 13
		dw 0
		ORG $200
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	
; parser states
;ParserExpectCommand	EQU 0
;ParserExpectCoordinate	EQU 1
;ParserInCoordinate	EQU 2

		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
		jmp Move ; 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
		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
		
		
		
				

	
;----------------------------- SUBROUTINES -----------------------------