'
'  stepper.bas     SBasic library file for controlling stepper motors
'
'  Created by:  Karl Lunt
'               Bothell, WA
'               karllunt@seanet.com
'

'
'  This file supports two stepper motors using separate L297/L298
'  controller boards.  It assumes the motors are wired as follows:
'
'            Left motor             Right motor
'  CLK          PB0                     PB3
'  CW/*CCW      PB1                     PB4
'  ENABLE       PB2                     PB5
'
'  This code also assumes that H/*F is pulled to +5 VDC (half-stepping),
'  RESET is pulled high, and the VREF control is hardwired to a value set
'  on the controller boards.  Note that a pair of port B lines remains
'  free, so you could modify this code to control one of those signals.
'

'
'  This code uses TOC4 and TOC5 to provide timing of motor steps.  Once
'  your code has called InitSteppers to initialize the stepper motor code,
'  it cannot modify the TOC4 and TOC5 subsystems, or the stepper operation
'  will break.
'

'
'  Your main source file MUST include regs11.lib before including this file!
'


declare  rsdly
declare  lsdly
declare  rscnt
declare  lscnt

const  RTOC = TOC4
const  LTOC = TOC5
const  RTOCV = $ffe2			' vector for right motor ISR
const  LTOCV = $ffe0			' vector for left motor ISR
const  RMASK = $10				' TMSK1 and TFLG1 mask for right motor
const  LMASK = $08				' TMSK1 and TFLG1 mask for left motor
const  RCLK1 = $08				' right motor clock line high
const  LCLK1 = $01				' left motor clock line high
const  RCLK0 = $f7				' right motor clock line low
const  LCLK0 = $fe				' left motor clock line low

'
'  The following CONST declarations set or clear specific motor control
'  lines.  Use these constants as AND and OR masks to create arguments for
'  the ConfigureSteppers routine below.
'
'  Masks ending in 1 are OR masks; use them in an OR operation to set the
'  desired bit.  Masks ending in 0 are AND masks; use them in an AND operation
'  to clear the desired bit.
'
'  For example, the invocation:
'
'      gosub ConfigureSteppers, RENB1 or LENB1 and RCW0 and LCW0
'
'  enables both motors and causes both motors to turn CCW (CW bits are low).
'

const  RENB1 = $20				' right motor enable line high
const  LENB1 = $04				' left motor enable line high
const  RENB0 = $df				' right motor enable line low
const  LENB0 = $fb				' left motor enable line low
const  RCW1 = $10				' right motor CW line high
const  LCW1 = $02				' left motor CW line high
const  RCW0 = $ef				' right motor CW line low
const  LCW0 = $fd				' left motor CW line low

'
'  The following consts can be used in changing specific motor parameters.
'

const  STEP_FWD = 3				' sets direction to forward
const  STEP_REV = 5				' sets direction to reverse


'
'  InitSteppers      set up the stepper controller software
'
'  Use this routine to set up the proper TOC subsystems for controlling
'  steppers.  You must invoke this routine before any other routines will
'  work.
'
'  After calling this routine, your code MUST enable interrupts before the
'  stepper code will work!
'

InitSteppers:
interrupts  off
rscnt = 0								' default count (0 = stopped)
lscnt = 0
rsdly = $fffe							' default delay (very long)
lsdly = $fffe
poke  RTOC, (peek(TCNT) + rsdly)		' time for next step
poke  LTOC, (peek(TCNT) + lsdly)
pokeb  TFLG1, (peekb(TFLG1) or RMASK or LMASK)	' clear any previous interrupts
pokeb  PORTB, (peekb(PORTB) and $c0)	' clear all stepper port lines
return


interrupt  $ffe2 							' right motor ISR
rtoc_isr:
pokeb  TFLG1, RMASK							' clear the interrupt flag
if rscnt <> 0								' if something to do...
	poke  RTOC, (peek(RTOC) + rsdly)		' set timer for next pulse
	if rscnt <> $ffff						' if not running forever...
		rscnt = rscnt - 1					' count this step
	endif	
	pokeb  PORTB, (peekb(PORTB) or RCLK1)	' bring clk high
	pokeb  PORTB, (peekb(PORTB) and RCLK0)	' now bring it low
endif
end


interrupt  	$ffe0							' left motor ISR
ltoc_isr:
pokeb  TFLG1, LMASK							' clear the interrupt flag
if lscnt <> 0								' if something to do...
	poke  LTOC, (peek(LTOC) + lsdly)		' set timer for next pulse
	if lscnt <> $ffff						' if not running forever...
		lscnt = lscnt - 1					' count this step
	endif	
	pokeb  PORTB, (peekb(PORTB) or LCLK1)	' bring clk high
	pokeb  PORTB, (peekb(PORTB) and LCLK0)	' now bring it low
endif
end



StartSteppers:
interrupts  off
poke  LTOC, (peek(TCNT) + $40)				' set time for next step soon
poke  RTOC, (peek(TCNT) + $40)
pokeb  TMSK1, (peekb(TMSK1) or RMASK or LMASK)		' allow TOC interrupts
pokeb  TFLG1, (peekb(TFLG1) or $18)			' clear any previous interrupts
interrupts on
return



StopSteppers:
interrupts  off
rscnt = 0								' default count (0 = stopped)
lscnt = 0
pokeb  TMSK1, (peekb(TMSK1) and (RMASK or LMASK xor $ffff))
interrupts  on
return



EnableSteppers:
interrupts off
pokeb  PORTB, (peekb(PORTB) or (RENB1 + LENB1))
interrupts on
return



DisableSteppers:
interrupts off
pokeb  PORTB, (peekb(PORTB) and (RENB0 and LENB0))
interrupts on
return



StepDirection:
if pick(0) = STEP_FWD						' test left direction
	pokeb  PORTB, (peekb(PORTB) and LCW0)
endif
if pull() = STEP_REV
	pokeb  PORTB, (peekb(PORTB) or LCW1)
endif
if pick(0) = STEP_FWD						' test right direction
	pokeb  PORTB, (peekb(PORTB) or RCW1)
endif
if pull() = STEP_REV
	pokeb  PORTB, (peekb(PORTB) and RCW0)
endif
return



GetRightStepCount:
return  rscnt



GetLeftStepCount:
return  lscnt




'
'  SetStepDelays      assign step delays for both stepper motors
'
'  This routine lets your code assign the time between step pulses for both
'  motors.  Invocation is:
'
'     gosub  SetStepDelays, LEFTDLY, RIGHTDLY
'
'  where LEFTDLY is the time between pulses for the left motor and RIGHTDLY
'  is the time between pulses for the right motor.  Time is measured in
'  TCNT units, which are normally 500 nsecs for a 68hc11 using an 8 MHz
'  crystal.
'
'  Any changes made to either step rate while the motors are running take
'  effect immediately.
'

SetStepDelays:
interrupts off
rsdly = pull()
lsdly = pull()
interrupts on
return



SetStepCounts:
interrupts off
rscnt = pull()
lscnt = pull()
interrupts on
return


'
'  End of the stepper library routines.  Insert your custom code
'  here.
'


'
'  The following code serves as the core to a Dead-reckoning robot.
'  You can modify the motion table at AMAP to describe any fixed
'  motion map you like.
'

'
'  The following constants give the number of steps per meter.
'

' const  METER = 1114			' s/m (full-stepping, 2.25" wheel)
' const  METER = 2228			' s/m (half-stepping, 2.25" wheel)
' const  METER = 911			' s/m (full-stepping, 2.75" wheel)
const  METER = 1822			' s/m (half-stepping, 2.75" wheel)

declare  done
declare  temp
declare  wait
declare  final
declare  index
declare  m1
declare  m2
declare  map

'
'  Declare a ROM-based table of motor motions.  Each entry in the
'  table contains 6 values, arranged as:
'
'  LDELAY, RDELAY					' step delays
'  LCOUNT, RCOUNT					' step counts
'  LDIR, RDIR						' direction
'
'  Mark the end of the table with a single entry containing $ffff.
'

amap:
data	10000,10000					' left and right step delays
data	4,4							' left and right step counts
data	STEP_FWD,STEP_FWD			' left and right directions

data	7500,7500
data	4,4		
data	STEP_FWD,STEP_FWD

data	5000,5000
data	METER*2-12,METER*2-12
data	STEP_FWD,STEP_FWD

data	10000,10000
data	4,4
data	STEP_FWD,STEP_FWD

data	5000,5000
data	136,136
data	STEP_FWD,STEP_REV

data	$ffff						' delay of $ffff marks end of table


interrupt $fff0						' RTI
if wait <> 0
	wait = wait - 1
endif
pokeb  tflg2, %01000000
end



main:
pokeb option, $80
pokeb  tflg2, %01000000				' clear pending RTI if any
pokeb  tmsk2, %01000000				' permit RTI interrupts
wait = 0
interrupts on
gosub InitSteppers
gosub EnableSteppers

wait = 125							' small startup delay (.5 secs)
do  loop while wait <> 0

map = addr(amap)
do
	m1 = peek(map)					' read the left step delay
	if m1 = $ffff					' $ffff means end of table
		exit
	endif
	map = map + 2
	m2 = peek(map)					' read the right step delay
	gosub SetStepDelays, m1, m2		' use table step delays

	map = map + 2
	m1 = peek(map)					' read the left step count
	map = map + 2
	m2 = peek(map)					' read the right step count
	gosub SetStepCounts, m1, m2

	map = map + 2
	m1 = peek(map)					' read the left direction
	map = map + 2
	m2 = peek(map)					' read the right direction
	gosub StepDirection, m1, m2

	map = map + 2
	gosub StartSteppers				' make the change
	do
	loop  until usr(GetRightStepCount) or usr(GetLeftStepCount)  = 0
loop
gosub DisableSteppers

end