*
*       Author: Alan Kilian   
*
*       Title : HC11 RC Servo routines
*
*       File Name : servo8.asm
*
*       Description : This program demonstrates the use of a HC11 processor
*		      to control 8 Model airplane type servos. The servos
*		      are updated every 20 milliseconds and their individual
*		      pulses do not jitter more than 1 microsecond.
*
*       History : 02/11/93 Created. Tested out one pulse.
*		  02/15/93 First cut of eight pulses.
*		  02/16/93 Getting eight good clean pulses.
*		  02/17/93 Added sort routine and tons of comments.
*		  02/17/93 Added user input routine.
*		  02/18/93 Debugged the whole thing and it was no fun at all
*	   	  02/18/93 Removed all the printing routines and debug stuff.
*		  02/20/93 Removed FCB from RAM area. Don't do that.
*
*	Use:	Send ASCII characters over the serial line to control the servos
*		The format is list this:  
*		s0000    sets servo 00 to pulse width $00 which is the minimum
*			 available pulse width.
*		s0080    sets servo 00 to pulse width $80 HEX which is the
*			 middle width pulse
*		s00FF    sets servo 00 to pulse width $FF HEX which is the 
*		 	 largest available pulse width
*		s0100    servo one
*		r        resets all servos to a pulse width $80
*
*       Note : This program is written for the MC68HC811E2 processor running
*	       in single-chip mode. It is designed for use with a processor
*	       running with an 8 mHz crystal. If you are using a different
*	       crystal frequency you will need to re-compute all of the
*	       timing values in this code.
*
*	       The structure, serial I/O and command processor portions of
*	       this program evolved from the program HEXLOB40 written by
*	       Fred Martin and Randy Sargent and we thank them greatly.
****************************************************************************

* Two-Byte Registers (High,Low -- Use Load & Store Double to access)
TOC2	EQU	$1018	; Timer Output Compare register 2

* One-Byte Registers
PORTB	EQU	$1004	; PORT B data register
TCTL1	EQU	$1020	; Timer ConTroL register 1
TCTL2	EQU	$1021	; Timer ConTroL register 2
TMSK1	EQU	$1022	; Timer interrupt MaSK register 1
TFLG1	EQU	$1023	; Timer interrupt FLaG register 1
RTFLG1	EQU	$23	; Relative to $1000 Timer interrupt FLaG register 1
TMSK2	EQU	$1024	; Timer interrupt MaSK register 2
TFLG2	EQU	$1025	; Timer interrupt FLaG register 2
SPCR	EQU	$1028	; SPI Control Register
BAUD	EQU	$102B	; SCI Baud Rate Control Register
SCCR2	EQU	$102D	; SCI Control Register 2
SCSR	EQU     $102E	; SCI Status Register (Actual location)
SCDR	EQU     $102F	; SCI Data Register (Actual location)


* Masks for serial port
PORTD_WOM	EQU	$20	; Wire-OR mode
BAUD1200	EQU	$B3	; 1200 Baud what else?
BAUD9600	EQU	$B0	; 9600 Baud what else?
TRENA		EQU	$0C	; Transmit, Receive ENAble
RDRF		EQU	$20	; Receive Data Register Full
TDRE		EQU	$80	; Transmit Data Register Empty

********************************************************************************
* zero page RAM definitions. Do not use FCB here. It will stomp EEBOOT20.
********************************************************************************

	ORG	$00		; The beginning of RAM
values  RMB	8		; The place where the unsorted values live
servnum	RMB	2		; A safe place for the servo number
smasks	RMB	8		; A safe place to put sorted masks
svalues	RMB	8		; A safe place to put sorted values
dvalues	RMB	8		; A safe place to put sorted delta values
newvals	RMB	1		; Alert the interrupt routine about new values

**********************************************************************
*			       MAIN CODE			     *
**********************************************************************

	ORG	$F800		; $F800 is the beginning of EEPROM
*				; on a MC68HC811E2 processor

Start:
	LDS	#$00FF		; Set stack at the top of ram

	BCLR	SPCR PORTD_WOM 	; initialize serial port
	LDAA	#BAUD9600	; turn off wire-or mode
	STAA	BAUD		; Set the port for 9600 baud
	LDAA	#TRENA
	STAA	SCCR2		; Enable the serial subsystem

	LDAA	#1
	STAA	newvals		; Set newvals the first time through

	LDAA	#0
	STAA	servnum		; Zero out the high byte of servnum

	LDX	#0		; set the servos to the middle
	LDAA	#$80		; of the range of pulse widths
Stlp:
	STAA	values,X
	INX
	CPX	#7
	BLS	Stlp

********************************************************************************
* set up interrupts
* OC2:  once every 20 milliseconds
********************************************************************************

	LDAA	#%01000000	; Set up the OC2 interrupt to generate
	STAA	TCTL1		; an interrupt once every 
	STAA	TFLG1 		; 20 milliseconds
	STAA	TMSK1
	CLI			; Turn on interrupts

mainloop:
	LDAA	#$0D		; return character
	JSR	putchar
	LDAA	#$0A		; linefeed character
	JSR	putchar
	LDAA	#'>		; prompt character
	JSR	putchar

cmd_get_type:
	JSR	getchar		; Keep getting chars until you get one
	CMPA	#'a		; that is alphabetic.
	BLO	cmd_get_type	; If the char was less than 'a' ignore

  	CMPA	#'s		; The command is an 's' set a new value
	BEQ	set_servo_val
	CMPA	#'r		; the command is an 'r' reset values
	BEQ     reset_servos
	BRA	mainloop	; Do it all over again

set_servo_val:
	JSR	getbyte		; Get the servo number
	STAA	servnum+1
	LDX	servnum		; And get it into the X register
	JSR	getbyte		; Get the pulse width
	STAA	values,X	; And save it in the values list
	LDAA	#1		; Alert the interrupt handler that there are
	STAA	newvals		; new values in the values list
	BRA	mainloop	; Go back to the command loop

reset_servos
	LDX	#0
rtop:
	LDAA	#$80		; Reset the servo values to a nice middle
	STAA	values,X	; pulse width
	INX
	CPX	#7
	BLS	rtop
    	BRA	mainloop	; Go back to the command loop

getbyte:
	JSR	getchar		; read 2 chars from the serial port and change
        CMPA    #'A		; them into a one byte value in accumulator A
        BLO     hibyteok	; This routine destroys accumulator B
        SUBA    #'A-10		; so watch it.
hibyteok
	ASLA
	ASLA
	ASLA
	ASLA
	TAB
	JSR	getchar		; Get the second character which is the least
        CMPA    #'A		; significant 4 bits of the value
        BLO     lobyteok
        SUBA    #'A-10
lobyteok
	ANDA	#$0f
	ABA
	RTS

getchar:
	LDAA	SCSR		; Read a character from the serial port and put
	ANDA	#RDRF		; it in accumulator A
        BEQ	getchar
	LDAA	SCDR
        ANDA    #$7f
	RTS

putchar:
	LDAB	SCSR		; Send the character in accumulator A out the
 	ANDB	#TDRE		; serial port.
	BEQ	putchar		; This routine destroys accumulator B
	STAA	SCDR		; so watch it.
	RTS

oc2int:
	LDD	#40000		; Once every 20 milliseconds
	ADDD	TOC2		; We need to generate an interrupt
	STD	TOC2
	LDX	#$1000
	BCLR	RTFLG1,X %10111111	; clear OC2 for next compare
	
********************************************************************************
* This section is timing critical. If you need to change anything in the
* oc2st loop you MUST make sure to get the delay in the oc2dn loop identical.
* There is a delay of 675 clocks to set the minimum pulse width. If you want
* to have a longer or shorter minimum pulse width you can change this value.
* The value 675 is derived as follows: The oc2st loop takes 39 clocks to
* start each pulse. Therefore it takes 7*39 clocks from the start of the first 
* pulse until the loop completes. The oc2dn loop takes 32 clocks until it stops
* the first pulse if that servo's values array holds a zero. This means that if
* we want a 950 clock (475 microsecond) minimum pulse we need to delay
* 950 - (7*39) - 32 = 645 clocks between the end of oc2st and the beginning
* of oc2dn. A DECA/BNE loop takes 5 clocks so we load A with 645/5 = 129.
* Now the LDAA #129 adds 2 clocks to the delay but do you REALLY care about
* a 2 clock difference in the desired minimum pulse width and the actual
* minimum pulse width?
* Do NOT replace it with an interrupt driven delay since this would introduce
* an unpredictable interrupt latency of as much as 41 clocks. This much change
* in the pulse width will almost surely cause the servos to jitter.
* Currently the oc2st loop takes 39 clocks as does the oc2dn loop.
* The pulse width changes 13 clocks for every count in the values array.
* This gives you a 13*256*500nanoSecond = 1664 microsecond change in pulse
* width from a zero to FF in the values array. If you add the 950 clock minimum
* pulse width to that you can produce pulses from 475 microseconds through
* 2139 microseconds with a resolution of 6.5 microseconds. Pretty good huh?
* The numbers after the semicolon are the number of clocks the instruction
* takes to execute. Branches take the same time even if the branch is not taken.
* One clock is 500 nanoseconds if you are using an 8 mHz crystal. If you are
* not using an 8 mHz crystal then all these timings are wrong. Too bad.
********************************************************************************
oc2st:
	LDX	#0		;3 Start at the shortest pulse-width
oc2st1:
	LDAB	smasks,X	;4 Figure out which servo it is
	ORAB	PORTB		;4
	STAB	PORTB		;4 Turn it on
	LDAA	#3		;2 We need to blow 15 clocks
oc2st2:
	DECA			;2
	BNE	oc2st2		;3
	INX			;3 Go to the next servo
	CPX	#7		;4
	BLS	oc2st1		;3 Not done, do another one.

	LDAA	#129		;2 Now, delay the minimum pulse-width
delay1:
	DECA			;2 Which is 645 clocks
	BNE	delay1		;3

oc2dn:
	LDX	#0		;3 Now start turning off the servo pulses
oc2dn1:
	LDAB	smasks,X	;4 Figure out which servo is first
	EORB	PORTB		;4 Figure out what to store in PORTB
	LDAA	dvalues,X	;4 Get this servos desired pulse width
oc2dn2:
	NOP			;2
	NOP			;2
	NOP			;2
	NOP			;2
	DECA			;2 And delay 13 clocks per unit
	BNE	oc2dn2		;3
	STAB	PORTB		;4 Finally turn off the pulse
	INX			;3 Go to the next servo
	CPX	#7		;4 Are we on the last servo?
	BLS	oc2dn1		;3 No, do another one

	LDAA	newvals		; Next see if there is a new values list
	CMPA	#0
	BEQ	done		; If not, we are done

	LDAA	#0
	STAA	newvals		; Zero out the new values indicator

	LDX	#0
	LDAB	#$01
oc2cp:
	LDAA	values,X	; Copy the values array
	STAA	svalues,X	; Into the svalues array
	STAB	smasks,X	; (Save a proper mask for this port)
	ASLB			; 
	INX			; So that we can sort them without
	CPX	#7		; worrying about getting new user
	BLS	oc2cp		; values in the values array

********************************************************************************
* Sort the svalues list so that the lowest pulse widths are at the beginning of
* the list. Also make sure to swap around the smasks list so that the proper
* masks stay with the values.
********************************************************************************
oc2sort:
	LDY	#0		; Y is our "times through the list" counter
sorttop:
	LDX	#0		; X is our pointer into the list of values
sortlp:
	LDAA	svalues,X	; Get a value
	LDAB	svalues+1,X	; Get the following value
	CBA			; Compare them
	BLS	noswap		; The first one is lower. No not swap them
	STAA	svalues+1,X	; Swap the two values
	STAB	svalues,X
	LDAA	smasks,X	; Also swap the bit masks
	LDAB	smasks+1,X
	STAA	smasks+1,X
	STAB	smasks,X
noswap:
	INX			; Go to the next entry in the list
	CPX	#6		; 6 is the second-to-last item and we are done
	BLS	sortlp		; Not done yet, do another pair of items
	INY
	CPY	#6		; We only need to sort 7 times so 6 is right
	BLS	sorttop

	LDX	#0		; Now convert from pulse width values
	LDAA	svalues,X
	INCA			; (Fix up for the delay loop)
	STAA	dvalues,X
oc2con:
	LDAA	svalues+1,X	; Into delta values so that we can simply
	SUBA	svalues,X	; delay the time left for each pulse
	INCA			; (Fix up for the delay loop)
	STAA	dvalues+1,X
	INX
	CPX	#6
	BLS	oc2con
done:
	RTI			; Done, now that didn't hurt too much did it?

BadInt	RTI			; Set all unused vectors here

	Org	$FFC0		; Where the interrupt vectors are

	FDB	BadInt	* $FFC0	; Reserved
	FDB	BadInt	* $FFC2	; Reserved
	FDB	BadInt	* $FFC4	; Reserved
	FDB	BadInt	* $FFC6	; Reserved
	FDB	BadInt	* $FFC8	; Reserved
	FDB	BadInt	* $FFCA	; Reserved
	FDB	BadInt	* $FFCC	; Reserved
	FDB	BadInt	* $FFCE	; Reserved
	FDB	BadInt	* $FFD0	; Reserved
	FDB	BadInt	* $FFD2	; Reserved
	FDB	BadInt	* $FFD4	; Reserved

	FDB	BadInt	* $FFD6	; SCI Serial System
	FDB	BadInt	* $FFD8	; SPI Serial Transfer Complete
	FDB	BadInt	* $FFDA	; Pulse Accumulator Input Edge
	FDB	BadInt	* $FFDC	; Pulse Accumulator Overflow
	FDB	BadInt	* $FFDE	; Timer Overflow
	FDB	BadInt	* $FFE0	; In Capture 4/Output Compare 5 (TI4O5)
	FDB	BadInt  * $FFE2	; Timer Output Compare 4 (TOC4)
	FDB	BadInt	* $FFE4	; Timer Output Compare 3 (TOC3) 
	FDB	oc2int	* $FFE6	; Timer Output Compare 2 (TOC2)
	FDB	BadInt	* $FFE8	; Timer Output Compare 1 (TOC1)
	FDB	BadInt	* $FFEA	; Timer Input Capture 3 (TIC3)
	FDB	BadInt	* $FFEC	; Timer Input Capture 2 (TIC2)
	FDB	BadInt	* $FFEE	; Timer Input Capture 1 (TIC1)
	FDB	BadInt	* $FFF0	; Real Time Interrupt (RTI)
	FDB	BadInt	* $FFF2	; External Pin or Parallel I/O (IRQ)
	FDB	BadInt	* $FFF4	; Pseudo Non-Maskable Interrupt (XIRQ)
	FDB	BadInt	* $FFF6	; Software Interrupt (SWI)
	FDB	BadInt	* $FFF8	; Illegal Opcode Trap ()
	FDB	BadInt	* $FFFA	; COP Failure (Reset) ()
	FDB 	BadInt	* $FFFC	; COP Clock Monitor Fail (Reset) ()
	FDB	Start	* $FFFE	; /RESET
	END
