*	OPT l,c,cre,s
*	File: s_stest.asm
*	Author: Tom Dickens
*	Slave - Servo test.asm
*
*	Initialize the servos, all 4, to their middle positions.
*	Through the SPI, look for the following sequence:
*	$AA $55 $nn $data $81
*	   Where $nn is the servo number (0 - 3) and data is
*	   a hex value from $00 to $FF.
*
*	RAM Variables:
SPI_POINTER	EQU	$00
SPI_DATA	EQU	$02
COUNT		EQU	$10
*
	ORG $B600		; Start of EEPROM
	jsr	Init_4_Servos
	ldd	#$0800		; Servo middle position
	std	$1018		; Initial setting for servo 0
	std	$101a		; Initial setting for servo 1
	std	$101c		; Initial setting for servo 2
	std	$101e		; Initial setting for servo 3
	jsr	Init_SPI
	ldd	#$0002
	std	SPI_POINTER
	clr  SPI_DATA+4
	clr  COUNT

Loop:
	ldaa	COUNT
	staa	$1004		; port B
	brclr	SPI_DATA+4 $FF Loop	; Wait for 5th SPI byte

	ldd	#$0002
	std	SPI_POINTER
	clr  SPI_DATA+4			; clear 5th byte
	ldd	SPI_DATA
	cpd	#$AA55
	bne	Loop
	ldab	SPI_DATA+2	; Which servo
	ldaa	SPI_DATA+3	; Servo data
	staa	$1004		; port B
	lslb
	ldy	#$1018		; Servo 2 data
	aby			; Y = Y + B
	tab
	clra
	lsld
	lsld
	lsld
	lsld
	lsld
	std	$00,y		; Write new servo value
	bra	Loop

*=============================================================
*
*
*  File:   initserv.asm
*  Author: Tom Dickens
*  Date:   June 4, 1995
*
*  Purpose: This file contains the servo initialization routines.
*
*  Subroutines:
*  Init_2_Servos: Initialize the software to drive two servo motors.
*  Init_4_Servos: Initialize the software to drive four servo motors.
*       Arguments:  -none-
*
*=============================================================
*  OK, what we want to do is to get a PWM signal going for a
*  servo motor.  The free-running counter will cycle at
*  500nS * 2^16 = 32.768 mS, which will work nicely
*  (Assuming 8 MHz Xtal).  We want a range from 1 mS to 2 ms
*  high, and the rest low.  Comparing to the counter values,
*  starting at $0000, 1 mS later we have $07D0, 1.5 mS at
*  $0BB8, and 2 mS at $0FA0 (decimal 2000, 3000, and 4000)
*  If we set the servo output(s) high at $0000 of the free-
*  running counter, we can use the 4 timer compare registers
*  to control bits 3, 4, 5, & 6 on port A.  Use the desired
*  value in the timer compare and configure the compare to
*  toggle the port bit.  This will happen automatically;
*  the port bits are set high when the free-running counter
*  rolls over, using OC1.  No interrupts are needed.
*
Init_4_Servos:
	psha
	pshx
	ldx     #$1000  ; set index to control registers
	ldaa    #%01010101
	staa    $20,x   ; set timers 2, 3, 4 & 5 to toggle at count equal
	ldaa    #%01111000
	staa    $0C,x   ; set timers 2, 3, 4 & 5 to set on OC1 overflow
	staa    $0D,x   ; specify data states for timers 2, 3, 4, & 5.
	pulx
	pula
	rts
*=============================================================
*=============================================================
*	OPT l,c,cre,s
*
*	Added SPI code as the SLAVE
*	Set up an SPI interrupt.  When data is received, echo it to portb.
*	Also sent the data back.
*========================================================================
*    Initialization:
*========================================================================
*	SPI initialization:
*========================================================================
Init_SPI:
CopyJMP:
*  copy the jump to the interrupt vector location for timer 1
	ldx	IntJmp
	stx	$100-57
	ldaa	IntJmp+2
	staa	$100-57+2

	ldaa	#%10101010
	staa	$1004

*	ldaa	#%00101111
*	staa	$1008		; PortD
	ldaa	#%00000100
	staa	$1009		; Data Direction for PortD (0=In, 1=Out)

	ldaa	#%11000100	; SLAVE
	staa	$1028

	cli			; Enable interrupts
	rts
*========================================================================
*	end of SPI initialization:
*========================================================================
*========================================================================
IntJmp	JMP	Interrupt	; Code to copy to RAM jump vector for SPI

*========================================================================
*========================================================================
Interrupt:
	inc	COUNT
	ldx	#$1000
	ldy	SPI_POINTER
SPIF_Not_Ready:
	brclr	$29,x $80 SPIF_Not_Ready
	ldaa	$102A		; GET the SPI data
	cpy	#$0002
	bne	Not_First_Data_Byte
	cmpa	#$AA
	bne	Bad_First_Data_Byte
Not_First_Data_Byte:
	staa	$00,y
	inc	SPI_POINTER+1
	
Bad_First_Data_Byte:
	rti
*    End of Interrupt
*========================================================================