littleinit
* do whatever init things, and include these
* the general timer is operating at it's fastest speed
* to setup servo $
TCTL1	EQU	$1020	; Timer Control register 1
TCTL2	EQU	$1021	; Timer Control register 2
TMSK1	EQU	$1022	; main Timer interrupt Mask register 1
TFLG1	EQU	$1023	; main Timer interrupt Flag register 1
TMSK2	EQU	$1024	; misc Timer interrupt Mask register 2
TFLG2	EQU	$1025	; misc Timer interrupt Flag register 2
PACTL	EQU	$1026	; Pulse Accumulator Control register
OC1M	EQU	$100C	; Output Compare 1 Mask register
	ldx	#$1000
	clra
	staa	TMSK2,x
	ldaa	#%11111000		;the 1's corrospond to OC1-5
	staa	TFLG1,x
	staa	TMSK1,x
	bset	TCTL1,x %01010101	; toggle at interrupt, OC2-4
	ldaa	#$80			; special OC1 setup
	staa	PACTL,x
	staa	OC1M,x
	cli

*the routines, can be anywhere in memory...
*do not execute! they are meant to be called
*from an interupt by the HC11 circutry
*if executed otherwise they will try to 
*return from an interupt that didn't happen-> (bad stack problem)
TOC1	EQU	$1016	; Timer Output Compare register 1
TOC2	EQU	$1018	; Timer Output Compare register 2
TOC3	EQU	$101A	; Timer Output Compare register 3
TOC4	EQU	$101C	; Timer Output Compare register 4
OC1M	EQU	$100C	; Output Compare 1 Mask register
OC1D	EQU	$100D	; Output Compare 1 Data register
handle_oc5:
	ldx	#$1000		;required for bclr
	bclr	TFLG1,x %11110111
	ldaa	0,x		;find level of PWM now
	anda	#%00001000
	beq	oc5pwmdown	;level is down
	ldd	$08		;load 'on' period from low mem
	bra	oc5pwmend
oc5pwmdown
	ldd	#28960		;load constant low period
oc5pwmend
	addd	TOC5,x		;add loaded period (hi or low)
	std	TOC5,x		;set next inerrupt
	rti			;all done
handle_oc4:
	ldx	#$1000		;a slightly different routine is needed
	bclr	TFLG1,x %11101111	;for each line. (different masks)
	ldaa	0,x
	anda	#%00010000
	beq	oc4pwmdown
	ldd	$06
	bra	oc4pwmend
oc4pwmdown
	ldd	#28960
oc4pwmend
	addd	TOC4,x
	std	TOC4,x
	rti
handle_oc3:
	ldx	#$1000
	bclr	TFLG1,x %11011111
	ldaa	0,x
	anda	#%00100000
	beq	oc3pwmdown
	ldd	$04
	bra	oc3pwmend
oc3pwmdown
	ldd	#28960
oc3pwmend
	addd	TOC3,x
	std	TOC3,x
	rti
handle_oc2:
	ldx	#$1000
	bclr	TFLG1,x %10111111
	ldaa	0,x
	anda	#%01000000
	beq	oc2pwmdown
	ldd	$02
	bra	oc2pwmend
oc2pwmdown
	ldd	#28960
oc2pwmend
	addd	TOC2,x
	std	TOC2,x
	rti
handle_oc1:
	ldx	#$1000
	bclr	TFLG1,x %01111111
	ldaa	0,x
	anda	#%10000000
	beq	oc1pwmdown
	ldd	$00
	bra	oc1pwmend
oc1pwmdown
	ldd	#28960
oc1pwmend
	addd	TOC1,x
	std	TOC1,x
	ldaa	OC1D,x
	eora	#$80
	staa	OC1D,x
	rti

*Copyright Brian Olson, 1995.  All rights reserved.
*Terms: Go ahead, use it. Acknowledge me. Use in a commercial product must be *negotiated.
*
email me
*http://www.andrew.cmu.edu/user/bolson/robot.html

*Interrupt dispatch table for routines
*adapted from Fred G. Martin's (fredm@media.mit.edu)   "HEXMON.ASM"
	org $ffe0
	FDB	handle_oc5	* $FFE0: Output Compare 5 (TOC5)
	FDB	handle_oc4	* $FFE2: Timer Output Compare 4 (TOC4)
	FDB	handle_oc3	* $FFE4: Timer Output Compare 3 (TOC3)
	FDB	handle_oc2	* $FFE6: Timer Output Compare 2 (TOC2)
	FDB	handle_oc1	* $FFE8: Timer Output Compare 1 (TOC1)