
*  Operational Parameters


_100MS          equ     25
_250MS          equ     61
_500MS          equ     125
_1SECOND        equ     250
_2SECONDS       equ     500
_3SECONDS       equ     750
_5SECONDS       equ     1250
_10SECONDS      equ     2500
_25SECONDS      equ     6250
_60SECONDS      equ     15000
_2MIN           equ     30000

RAM     	equ     $0800           ;68HC812A4 internal RAM
STACK   	equ     $0bff           ;Stack at top of internal ram
rbase   	equ     $0000           ;68HC812A4 register block
EEPROM  	equ     $f000           ;68HC812A4 internal EEPROM
CONSTANTS	equ	$fe00		;CONSTANTS STORED IN EEPROM

* Operational Constants

TRUE            equ     $FF
FALSE           equ     $00
CR              equ     $D
LF              equ     $A
SPACE           equ     $20

;RTI Variables
clrmask         equ     %11000000       ;mask for clearing timer flags
rtimask1        equ     %10000001       ;M=8Mhz, 1.024 msec interrupt with 16 MHz xtal
rtimask2        equ     %10000010       ;M=8Mhz, 2.048 msec interrupt with 16 MHz xtal
rtimask3        equ     %10000011       ;M=8Mhz, 4.096 msec interrupt with 16 MHz xtal
rtimask4        equ     %10000100       ;M=8Mhz, 8.196 msec interrupt with 16 MHz xtal
rtimask5        equ     %10000101       ;M=8Mhz, 16.384 msec interrupt with 16 MHz xtal
rtimask6        equ     %10000110       ;M=8Mhz, 32.768 msec interrupt with 16 MHz xtal
rtimask7        equ     %10000111       ;M=8Mhz, 65.536 msec interrupt with 16 MHz xtal
rtiflag         equ     %10000000

;SCI Variables
scimask         equ     %00101100       ;RIE - SCI Interrupt enable
                                        ;RE - Receiver Enable
RDRFflag        equ     %00100000       ;RDRF - Receive Data Register Full flag
TDREflag        equ     %10000000       ;TDRE - Transmit Data Register Empty flag

;Baud rate definitions
;MCLK=8Mzh
BAUD110         equ     4545    ;(baud) 110 baud with 16 Mhz crystal
BAUD300         equ     1667    ;(baud) 300 baud with 16 Mhz crystal
BAUD600         equ     833     ;(baud) 600 baud with 16 Mhz crystal
BAUD1200        equ     417     ;(baud) 1200 baud with 16 Mhz crystal
BAUD2400        equ     208     ;(baud) 2400 baud with 16 Mhz crystal
BAUD4800        equ     104     ;(baud) 4800 baud with 16 Mhz crystal
BAUD9600        equ     52      ;(baud) 9600 baud with 16 Mhz crystal
BAUD14400       equ     35      ;(baud) 14400 baud with 16 Mhz crystal
BAUD19200       equ     26      ;(baud) 19200 baud with 16 Mhz crystal
BAUD38400       equ     13      ;(baud) 38400 baud with 16 Mhz crystal


* Registers
REG     EQU     $0000

PORTA   EQU     $0000           ;PORTA
PORTB   EQU     $0001           ;PORTB
DDRA    EQU     $0002           ;PORTA - DATA DIRECTION REGISTER
DDRB    EQU     $0003           ;PORTB - DATA DIRECTION REGISTER
PORTC   EQU     $0004           ;PORTC
PORTD   EQU     $0005           ;PORTD
DDRC    EQU     $0006           ;PORTC - DATA DIRECTION REGISTER
DDRD    EQU     $0007           ;PORTD - DATA DIRECTION REGISTER
PORTE   EQU     $0008           ;PORTE
DDRE    EQU     $0009           ;PORTE - DATA DIRECTION REGISTER
PEAR    EQU     $000A           ;PEAR - PORTE ASSIGNMENT REGISTER
MODE    EQU     $000B           ;MODE - MODE REGISTER
PUCR    EQU     $000C           ;PUCR - PULL UP CONTROL REGISTER
RDRIV   EQU     $000D           ;RDRIV - REDUCED DRIVE OF I/O LINES


INITRM  EQU     $0010           ;INITRM - INITIALIZATION OF INTERNAL RAM POSITION REGISTER
INITRG  EQU     $0011           ;INITRG - INITIALIZATION OF INTERNAL REGISTER POSITION REGISTER
INITEE  EQU     $0012           ;INITEE - INITIALIZATION OF INTERNAL EEPROM POSITION REGISTER
MISC    EQU     $0013           ;MISC - MISCELLANEOUS MAPPING CONTROL REGISTER
RTICTL  EQU     $0014           ;RTICTL - REAL TIME INTERRUPT CONTROL REGISTER
RTIFLG  EQU     $0015           ;RTIFLG - REAL TIME INTERRUPT FLAG REGISTER
COPCTL  EQU     $0016           ;COPCTL - COP CONTROL REGISTER
COPRST  EQU     $0017           ;COPRST - ARM/RESET COP TIMER REGISTER
ITST0   EQU     $0018           ;ITST0
ITST1   EQU     $0019           ;ITST1
ITST2   EQU     $001A           ;ITST2
ITST3   EQU     $001B           ;ITST3

INTCR   EQU     $001E           ;INTCR - INTERRUPT CONTROL REGISTER
HPRIO   EQU     $001F           ;HPRIO - HIGHEST PRIORITY I INTERRRUPT
KWIED   EQU     $0020           ;KWIED - KEY WAKEUP PORTD INTERRUPT ENABLE REGISTER
KWIFD   EQU     $0021           ;KWIFD - KEY WAKEUP PORTD FLAG REGISTER

PORTH   EQU     $0024           ;PORTH
DDRH    EQU     $0025           ;DDRH - DATA DIRECTION REGISTER
KWIEH   EQU     $0026           ;KWIEH - KEY WAKEUP PORTH INTERRUPT ENABLE REGISTER
KWIFH   EQU     $0027           ;KWIFH - KEY WAKEUP PORTH FLAG REGISTER
PORTJ   EQU     $0028           ;PORTJ
DDRJ    EQU     $0029           ;DDRJ DATA DIRECTION REGISTER
KWIEJ   EQU     $002A           ;KWIEJ - KEY WAKEUP PORTJ INTERRUPT ENABLE REGISTER
KWIFJ   EQU     $002B           ;KWIFJ - KEY WAKEUP PORTJ FLAG REGISTER
KPOLJ   EQU     $002C           ;KPOLJ - KEY WAKEUP PORTJ POLARITY REGISTER
PUPSJ   EQU     $002D           ;PUPSJ - KEY WAKEUP PORTJ PULL-UP/PULLDOWN SELECT REGISTER
PULEJ   EQU     $002E           ;PULEJ - KEY WAKEUP PORTJ PULL-UP/PULLDOWN ENABLE REGISTER

PORTF   EQU     $0030           ;PORTF
PORTG   EQU     $0031           ;PORTG
DDRF    EQU     $0032           ;DDRF - DATA DIRECTION REGISTER
DDRG    EQU     $0033           ;DDRG - DATA DIRECTION REGISTER
DPAGE   EQU     $0034           ;DPAGE - DATA PAGE REGISTER
PPAGE   EQU     $0035           ;PPAGE - PROGRAM PAGE REGISTER
EPAGE   EQU     $0036           ;EPAGE - EXTRA PAGE REGISTER
WINDEF  EQU     $0037           ;WINDEF - WINDOW DEFINATION REGISTER
MXAR    EQU     $0038           ;MXAR - MEMORY EXPANSION ASSGNMENT REGISTER

CSCTL0  EQU     $003C           ;CSCTL0 - CHIP SELECT CONTROL REGISTER 0
CSCTL1  EQU     $003D           ;CSCTL1 - CHIP SELECT CONTROL REGISTER 1
CSSTR0  EQU     $003E           ;CSSTR0 - CHIP SELECT STRETCH REGISTER 0
CSSTR1  EQU     $003F           ;CSSTR1 - CHIP SELECT STRETCH REGISTER 1
LDVH    EQU     $0040           ;LDV - LOOP DIVIDER HIGH REGISTER
LDVL    EQU     $0041           ;LDV - LOW REGISTER
RDVH    EQU     $0042           ;RDV - REFERENCE DIVIDER HIGH REGISTER
RDVL    EQU     $0043           ;RDV - LOW REGISTER

CLKCTL  EQU     $0047           ;CLKCTL - CLOCK CONTROL REGISTER

ATDCTL0 EQU     $0060           ;ATDCTL0 - RESERVED
ATDCTL1 EQU     $0061           ;ATDCTL1 - RESERVED
ATDCTL2 EQU     $0062           ;ATDCTL2 - ATD CONTROL REGISTER
ATDCTL3 EQU     $0063           ;ATDCTL3 - ATD CONTROL REGISTER
ATDCTL4 EQU     $0064           ;ATDCTL4 - ATD CONTROL REGISTER
ATDCTL5 EQU     $0065           ;ATDCTL5 - ATD CONTROL REGISTER

ATDSTAT         EQU     $0066
ATDSTATH        EQU     $0066   ;ATDSTAT - ATD STATUS HIGH REGISTER
ATDSTATL        EQU     $0067   ;ATDSTAT - LOW REGISTER

STDTEST         EQU     $0068
ATDTESTH        EQU     $0068   ;ATDTEST - ATD TEST HIGH REGISTER
ATDTESTL        EQU     $0069   ;ATDTEST - LOW REGISTER

PORTAD  EQU     $006F           ;PORTAD - PORT AD DATA INPUT REGISTER
ADR0H   EQU     $0070           ;ADR0H
ADR1H   EQU     $0072           ;ADR1H
ADR2H   EQU     $0074           ;ADR2H
ADR3H   EQU     $0076           ;ADR3H
ADR4H   EQU     $0078           ;ADR4H
ADR5H   EQU     $007A           ;ADR5H
ADR6H   EQU     $007C           ;ADR6H
ADR7H   EQU     $007E           ;ADR7H

TIOS    EQU     $0080           ;TIOS - TIMER INPUT CAPTURE/OUTPUT COMPARE SELECT
CFORC   EQU     $0081           ;CFORC - TIMER COMPARE FORCE REGISTER
OC7M    EQU     $0082           ;OC7M - OUTPUT COMPARE 7 MASK REGISTER
OC7D    EQU     $0083           ;OC7D - OUTPUT COMPARE 7 DATA REGISTER

TCNT    EQU     $0084
TCNTH   EQU     $0084           ;TCNT - TIMER COUNT HIGH REGISTER
TCNTL   EQU     $0085           ;TCNT - HIGH REGISTER

TSCR    EQU     $0086           ;TSCR - TIMER SYSTEM CONTROL REGISTER
TQCR    EQU     $0087           ;TQCR - RESERVED
TCTL1   EQU     $0088           ;TCTL1 - TIMER CONTROL REGISTER 1
TCTL2   EQU     $0089           ;TCTL2 - TIMER CONTROL REGISTER 2
TCTL3   EQU     $008A           ;TCTL3 - TIMER CONTROL REGISTER 3
TCTL4   EQU     $008B           ;TCTL4 - TIMER CONTROL REGISTER 4
TMSK1   EQU     $008C           ;TMSK1 - TIMER INTERRUPT MASK 1
TMSK2   EQU     $008D           ;TMSK2 - TIMER INTERRUPT MASK 2
TFLG1   EQU     $008E           ;TFLG1 - TIMER INTERRUPT FLAG 1
TFLG2   EQU     $008F           ;TFLG2 - TIMER INTERRUPT FLAG2

TC0     EQU     $0090
TC0H    EQU     $0090           ;TC0 - TIMER INPUT/CAPTURE COMPARE HIGH REGISTER0
TC0L    EQU     $0091           ;TC0 - LOW REGISTER

TC1     EQU     $0092
TC1H    EQU     $0092           ;TC1 - HIGH REGISTER
TC1L    EQU     $0093           ;TC1 - LOW REGISTER

TC2     EQU     $0094
TC2H    EQU     $0094           ;TC2 - HIGH REGISTER
TC2L    EQU     $0095           ;TC2 - LOW REGISTER

TC3     EQU     $0096
TC3H    EQU     $0096           ;TC3 - HIGH REGISTER
TC3L    EQU     $0097           ;TC3 - LOW REGISTER

TC4     EQU     $0098 
TC4H    EQU     $0098           ;TC4 - HIGH REGISTER
TC4L    EQU     $0099           ;TC4 - LOW REGISTER

TC5     EQU     $009A
TC5H    EQU     $009A           ;TC5 - HIGH REGISTER
TC5L    EQU     $009B           ;TC5 - LOW REGISTER

TC6     EQU     $009C
TC6H    EQU     $009C           ;TC6 - HIGH REGISTER
TC6L    EQU     $009D           ;TC6 - LOW REGISTER

TC7     EQU     $009E
TC7H    EQU     $009E           ;TC7 - HIGH REGISTER
TC7L    EQU     $009F           ;TC7 - LOW REGISTER

PACTL   EQU     $00A0           ;PATCL - PULSE ACCUMULATOR CONTROL REGISTER
PAFLG   EQU     $00A1           ;PAFLG - PULSE ACCUMULATOR FLAG REGISTER

PACNT   EQU     $00A2
PACNTH  EQU     $00A2           ;PACNT - 16 BIT PULSE ACCUMULATOR COUNT HIGH REGISTER
PACNTL  EQU     $00A3           ;PACNT - LOW REGISTER

TIMTST  EQU     $00AD           ;TIMTST - TIMER TEST REGISTER
PORTT   EQU     $00AE           ;PORTT
DDRT    EQU     $00AF           ;PORTT - DATA DIRECTION REGISTER

SC0BDH  EQU     $00C0           ;SC0BDH - SCI BAUD RATE CONTROL REGISTER
SC0BDL  EQU     $00C1           ;SC0BDL - SCI BAUD RATE CONTROL REGISTER
SC0CR1  EQU     $00C2           ;SC0CR1 - SCI CONTROL REGISTER
SC0CR2  EQU     $00C3           ;SC0CR2 - SCI CONTROL REGISTER
SC0SR1  EQU     $00C4           ;SC0SR1 - SCI STATUS REGISTER
SC0SR2  EQU     $00C5           ;SC0SR2 - SCI STATUS REGISTER
SC0DRH  EQU     $00C6           ;SC0DRH - SCI DATA REGISTER
SC0DRL  EQU     $00C7           ;SC0DRL - SCI DATA REGISTER
SC1BDH  EQU     $00C8           ;SC1BDH - SCI BAUD RATE CONTROL REGISTER
SC1BDL  EQU     $00C9           ;SC1BDL - SCI BAUD RATE CONTROL REGISTER
SC1CR1  EQU     $00CA           ;SC1CR1 - SCI BAUD CONTROL REGISTER
SC1CR2  EQU     $00CB           ;SC1CR2 - SCI CONTROL REGISTER
SC1SR1  EQU     $00CC           ;SC1SR1 - SCI STATUS REGISTER
SC1SR2  EQU     $00CD           ;SC1SR2 - SCI STATUS REGISTER
SC1DRH  EQU     $00CE           ;SC1DRH - SCI DATA REGISTER
SC1DRL  EQU     $00CF           ;SC1DRL - SCI DATA REGISTER
SP0CR1  EQU     $00D0           ;SP0CR1 - SPI CONTROL REGISTER
SP0CR2  EQU     $00D1           ;SP0CR2 - SPI CONTROL REGISTER
SP0BR   EQU     $00D2           ;SP0BR - SPI BAUD RATE REGISTER
SP0SR   EQU     $00D3           ;SP0SR - SPI STATUS REGISTER

SP0DR   EQU     $00D5           ;SP0DR - SPI DATA REGISTER
PORTS   EQU     $00D6           ;PORTS
DDRS    EQU     $00D7           ;PORTS - DATA DIRECTION REGISTER

EEMCR   EQU     $00F0           ;EEMCR - EEPROM MODULE CONFIGURATION
EEPROT  EQU     $00F1           ;EEPROT - EEPROM BLOCK PROTECT
EETST   EQU     $00F2           ;EETST - EEPROM TEST
EEPROG  EQU     $00F3           ;EEPROG - EEPROM CONTROL

*MASKS
BIT0		EQU	%00000001
BIT1		EQU	%00000010
BIT5		EQU	%00100000
BIT7		EQU	%10000000
BIT76		EQU	%11000000
BIT41		EQU	%00010010
BIT32		EQU	%00001100


;		ORG	CONSTANTS
;MENU		DC.B	"SELECT AN OPTION:", LF, "1)TURN SERVO LEFT", LF, "2)TURN SERVO RIGHT", LF, "3)CENTER SERVO", 0


************************************************
*  MAIN PROGRAM
************************************************

		ORG	EEPROM
		
		LDS	#STACK
		
		JSR	INIT_SCI	;INIT SCI SYSTEM
		JSR	INIT_MOTOR	;INIT MOTORS
		JSR	INIT_SERVOS	;INIT SERVOS
		JSR	INIT_AD		;INIT AD SYSTEM
		CLI

;Initialize PORTT - LED on board
     		movb    #%01100000, DDRT         ;Bit 6,5 of PORTT are Output
     		bclr    PORTT, %01100000

;Initialize PORTA
		movb	#%00000011, DDRA	 ;Bit 0,1 of PORTA are Output
		bclr	PORTT, %00000011

;Motors foward
		movb	#%00000101, PORTJ


LOOP		LDAA	SC0SR1		; check status reg (RDRF in bit 5)
		ANDA	#%00100000	; check if receive buffer full
		BEQ	LOOP		; wait until data present

		LDAB	SC0DRL
		CMPB	#0	; first byte = 0 means following byte is for left motor 
		BEQ	LEFT_MOTOR

		CMPB	#1	; first byte = 1 means following byte is for right motor 
		BEQ	RIGHT_MOTOR

		CMPB	#2	; first byte = 2 means following byte is direction
		BEQ	DIRECTION

		CMPB	#3	; first byte = 3 means following byte is for up/down servo
		BEQ	UP_DOWN

		CMPB	#4	; first byte = 4 means following byte is for left/right servo 
		BEQ	LEFT_RIGHT
	
		CMPB	#5	; first byte = 5 means send sensor readings
		LBEQ	GET_READINGS
				
		CMPB	#6	;Turn on camera or toggle Mode on camera
		LBEQ	TURN_ONCAM

		CMPB	#7
		LBEQ	CAPTURE_PIC

		BRA	LOOP

LEFT_MOTOR	LDAA	SC0SR1		; check status reg (RDRF in bit 5)
		ANDA	#%00100000	; check if receive buffer full
		BEQ	LEFT_MOTOR	; wait until data present
		
		LDAB	SC0DRL
		LDAA	#0
		LDY	#400
		EMUL			;multiply command by 400 and add 1
		ADDD	#1
		STD	TC0
		
		BRA	LOOP
		
RIGHT_MOTOR	LDAA	SC0SR1		; check status reg (RDRF in bit 5)
		ANDA	#%00100000	; check if receive buffer full
		BEQ	RIGHT_MOTOR	; wait until data present

		LDAB	SC0DRL
		LDAA	#0
		LDY	#400
		EMUL
		ADDD	#1
		STD	TC1
		
		BRA 	LOOP

DIRECTION	LDAA	SC0SR1		; check status reg (RDRF in bit 5)
		ANDA	#%00100000	; check if receive buffer full
		BEQ	DIRECTION	; wait until data present
		
		LDAB	SC0DRL	
		BEQ	FORWARD
		
		movb	#%00001010, PORTJ
		BRA 	LOOP	
	
FORWARD		movb	#%00000101, PORTJ
		LBRA	LOOP

		
UP_DOWN		LDAA	SC0SR1		; check status reg (RDRF in bit 5)
		ANDA	#%00100000	; check if receive buffer full
		BEQ	UP_DOWN 	; wait until data present

		LDAB	SC0DRL
		LDAA	#0
		LDY	#14		; multiply by constant
		EMUL
		ADDD	#1400
		STD	TC2
		
		LBRA 	LOOP

LEFT_RIGHT	LDAA	SC0SR1		; check status reg (RDRF in bit 5)
		ANDA	#%00100000	; check if receive buffer full
		BEQ	LEFT_RIGHT 	; wait until data present

		LDAB	SC0DRL
		LDAA	#0
		LDY	#14		; multiply by constant
		EMUL
		ADDD	#1400
		STD	TC3
		
		LBRA 	LOOP

GET_READINGS	LDAA	ADR0H		; send IR right
		JSR	SEND_BYTE
		LDAA	ADR1H		; send IR left
		JSR	SEND_BYTE
		LDAA	PORTG		; send bump sensors
		JSR	SEND_BYTE
		LDAA	PORTJ		; send pyro
		JSR	SEND_BYTE

		LBRA	LOOP

TURN_ONCAM	movb	#%00000010, PORTA
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		movb	#%00000000, PORTA
	

		LBRA	LOOP

CAPTURE_PIC	movb	#%00000001, PORTA
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		JSR	WAIT_HALF
		movb	#%00000000, PORTA
		

		LBRA	LOOP

*-----------------------------------------------
*  SUBROUTINES
*-----------------------------------------------

*-----------------------------------------------------------
**  Send whatever is in accumulator A out the serial port
**     when it becomes available.
*-----------------------------------------------------------

SEND_BYTE	BRCLR	SC0SR1, %10000000, SEND_BYTE
		STAA	SC0DRL
		RTS


************************************************
*  Prints out to SCI until EOF (0) is reached by 
*     where X is pointing
************************************************

PRINT_SCI	BRCLR	SC0SR1, BIT7, PRINT_SCI		; Wait for TDR to be empty
		MOVB	1, X+, SC0DRL			; Send out char and inc X
		TST	0, X
		BNE	PRINT_SCI			; If not end of string, continue
		RTS					; Otherwise, end subroutine

************************************************
*  Prints single char in REG B to SCI
************************************************
PRINT_CHAR	TST	SC0SR1  		;wait for transmit data register empty (TDRE)
       		BPL     PRINT_CHAR		; Wait for TDR to be empty
		STAB	SC0DRL				; Send Out
		RTS					; End subroutine

************************************************
**  DELAY ROUTINE  
**      REGISTERS: Y Reg 
**      5ms = 12(0.5us) + 6(0.5us)Y - 2(0.5us)
************************************************

DELAY		PSHY			;Y REG IS USED, SO SAVING FIRST
		LDY	#200		;LOADING Y WITH TIME DELAY
IN_DELAY	NOP
       		NOP
       		DEY
       		BNE	IN_DELAY
       		PULY			;PULLING Y TO WHAT IT WAS ORIGNIALLY
		RTS


*-----------------------------------------------------------
**  DELAY ROUTINE  
**      REGISTERS: Y Reg 
**      500us = 12(0.125us) + 6(0.125us)Y - 2(0.125us)
*-----------------------------------------------------------

WAIT_HALF	PSHY			;Y REG IS USED, SO SAVING FIRST
		LDY	#$FFFF		;LOADING Y WITH TIME DELAY
IN_HALF		NOP
       		NOP
       		DEY
       		BNE	IN_HALF
       		PULY			;PULLING Y TO WHAT IT WAS ORIGNIALLY
		RTS


*-----------------------------------------------------------
**  DELAY ROUTINE  
**      REGISTERS: Y Reg 
**      100us = 12(0.125us) + 6(0.125us)Y - 2(0.125us)
*-----------------------------------------------------------

WAIT_AD		PSHY			;Y REG IS USED, SO SAVING FIRST
		LDY	#132		;LOADING Y WITH TIME DELAY
IN_WAIT_AD	NOP
       		NOP
       		DEY
       		BNE	IN_WAIT_AD
       		PULY			;PULLING Y TO WHAT IT WAS ORIGNIALLY
		RTS

**************************************************
*  INITIALIZATION ROUTINES
**************************************************

*-------------------------------------------------
*   INIT SCI SYSTEM
*-------------------------------------------------

INIT_SCI	movb    #0,SC0CR2         ;disable SCI 0 rcvr. & xmtr. & rx int    
       		movb    #0,SC0CR1    
       		movb    #BAUD9600, SC0BDL        ;Set baud rate to 9600  

		LDAA	#%00001100	; Enable Tx and Rx;
		STAA	SC0CR2		;   all interrupts diabled
  
     		ldaa    SC0SR1                  ;read register to clear flag RDRF
     		ldaa    SC0DRL                  ;dummy read to flush receive buffer

******************End SCI Init*******************


*-------------------------------------------------
*   INIT MOTORS (only on OCO and OC1)
*-------------------------------------------------



INIT_MOTOR	BSET	tios,%10000011	; Setup channels 0,1, and 7 to be TOC channels.

; Set channels 0 and 1 to clear their output pins when the compare happens

		BSET	tctl2, %00001010

; Set the prescalar to roll over on 32ms periods
	
		BSET	tmsk2,%00110010

; Setup TOC7 to handle the start of the pulses by setting the value to 1
; when the TCNT is zero

		BSET	oc7m, %00000011
		BSET	oc7d, %00000011
		LDX	#$9C40
		STX	TC7
		BSET	tmsk2, %00001000
; Turn on the timer
		BSET	tscr, %10000000

;Initialize PORTJ
		movb	#%00001111,DDRJ		;Bits 0 - 3 of PORTJ are Output
		MOVW	#1, tc0
		MOVW	#1, tc1
		RTS		;END SERVICE ROUTINE

******************End Motor Init*******************

*-------------------------------------------------
*   INIT SERVOS (only on OC2 and OC3)
*-------------------------------------------------



INIT_SERVOS	BSET	tios,%10001100	; Setup channels 2,3 and 7 to be TOC channels.

; Set channels 2 and 3 to clear their output pins when the compare happens

		BSET	tctl2, %10100000

; Set the prescalar to roll over on 32ms periods
	
		BSET	tmsk2,%00110010

; Setup TOC7 to handle the start of the pulses by setting the value to 1
; when the TCNT is zero

		BSET	oc7m, %00001100
		BSET	oc7d, %00001100
		LDX	#$9C40
		STX	TC7
		BSET	tmsk2, %00001000
; Turn on the timer
		BSET	tscr, %10000000

		MOVW	#1820, tc2
		MOVW	#3276, tc3

		RTS		;END SERVICE ROUTINE

******************End Servo Init*******************

*-----------------------------------------------------------
**  Initialize Analog to Digital converter
*-----------------------------------------------------------

INIT_AD		movb	#%10000000, ATDCTL2
		jsr	WAIT_AD			;wait 100us for AD to be ready
		movb 	#%00110000, ATDCTL5
		RTS				;end subroutine

******************End AD Init*******************

*MC68HC812A4 VECTOR INTERRUPTS

VECTOR  EQU     $FFCE
        ORG     VECTOR

        FDB     EEPROM          ;KEY WAKEUP H
        FDB     EEPROM          ;KEY WAKEUP J
        FDB     EEPROM          ;ANALOG TO DIGITAL
        FDB     EEPROM          ;SERIAL COMMUNICATION 1
        FDB     EEPROM          ;SERIAL COMMUNICATION 0
        FDB     EEPROM          ;SPI SERIAL TRANSFER COMPLETE
        FDB     EEPROM          ;PULSE ACCUMULATOR INPUT EDGE
        FDB     EEPROM          ;PULSE ACCUMULATOR OVERFLOW
        FDB     EEPROM          ;TIMER OVERFLOW
        FDB     EEPROM          ;TIMER CHANNEL 7
        FDB     EEPROM          ;TIMER CHANNEL 6
        FDB     EEPROM          ;TIMER CHANNEL 5
        FDB     EEPROM          ;TIMER CHANNEL 4
        FDB     EEPROM          ;TIMER CHANNEL 3
        FDB     EEPROM          ;TIMER CHANNEL 2
        FDB     EEPROM          ;TIMER CHANNEL 1
        FDB     EEPROM          ;TIMER CHANNEL 0
        FDB     EEPROM          ;REAL TIME INTERRUPT
        FDB     EEPROM          ;IRQ OR KEY WAKE UP D
        FDB     EEPROM          ;XIRQ
        FDB     EEPROM          ;SWI
        FDB     EEPROM          ;RESERVED
        FDB     EEPROM          ;COP FAILURE RESET
        FDB     EEPROM          ;COP CLOCK MONITOR FAIL RESET
        FDB     EEPROM          ;RESET