*	OPT l,c,cre,s
*========================================================================
*    Jump to Start:  This is used to put in tables before the code.
	ORG $B600		; Start of EEPROM
	bra	Start
*========================================================================
*	File: m_stest.asm : master servo test.
*	Author: Tom Dickens
*
*	This code will read 8 pots on port E (A/D) and report their
*	values on an LCD.  The values will also be sent to slaves
*	using the SPI.  The slaves will control eight servos.
*
*	   Top View of the Robot with servo designations.
*
*	     S3:FR-Leg                       S7:BR-Leg
*	   +-------                      --------+
*	   |     /                         \     |
*	   |  +/-----------------------------\+  |
*	   | /|S2:FR-Sholder     S6:BR-Sholder|\ |
*	      |                               |
*	Front |                               |  Back
*	      |                               |
*	      |                               |
*	   | \|S0:FL-Sholder     S4:Bl-Sholder|/ |
*	   |  +\-----------------------------/+  |
*	   |     \                         /     |
*	   +-------                      --------+
*	     S1:FL-Leg                       S5:BL-Leg
*
*	Added SPI code as the MASTER
*	3 slaves are used, selected from bits 1, 2, and 3 from port B
*	Low = slave selected.  Only 1 slave should be selected at a time.
*
*	Tested values for the 8 servos:
*
*  shoulders:	90deg	toe-in	toe-out
*	S0:	54	60	40
*	S2:	90	82	A2
*	S4:	90	82	A2
*	S6:	54	60	40
*	Legs:	90deg	front30	back30	front90	back90
*	S1:	6A	80	5C	A0	34
*	S3:	7C	6E	8C	48	B0
*	S5:	6A	80	5C	A0	34
*	S7:	6A	60	7A	35	A0
*
*========================================================================
*	tables of values for servos
*========================================================================
STAND_TABLE:
 fcb	$54, $6A, $90, $7C, $90, $6A, $54, $6A
FORWARD_30_TABLE:
*		the leg forward
 fcb	$54, $74, $90, $76, $90, $70, $54, $64
BACK_30_TABLE:
 fcb	$54, $64, $90, $82, $90, $64, $54, $70
*
*        1111111   2222222   3333333   4444444
**2  0
**3  1  works OK
WALK_0_TABLE:
 fcb	$54, $74, $90, $81, $A2, $64, $54, $68
WALK_1_TABLE:
 fcb	$54, $70, $90, $82, $A2, $70, $54, $6A
WALK_2_TABLE:
 fcb	$54, $6D, $A2, $82, $90, $70, $54, $6D
WALK_3_TABLE:
 fcb	$54, $6A, $A2, $76, $90, $6D, $54, $70
WALK_4_TABLE:
 fcb	$54, $67, $90, $76, $90, $6A, $40, $70
WALK_5_TABLE:
 fcb	$54, $64, $90, $79, $90, $68, $40, $64
WALK_6_TABLE:
 fcb	$40, $64, $90, $7C, $90, $66, $54, $64
WALK_7_TABLE:
 fcb	$40, $74, $90, $7F, $90, $64, $54, $66
**
**
*** This is the proper table, but not too good.
*WALK_0_TABLE:
* fcb	$54, $6D, $A2, $82, $90, $66, $54, $64
*WALK_1_TABLE:
* fcb	$54, $6A, $A2, $76, $90, $64, $54, $66
*WALK_2_TABLE:
* fcb	$54, $67, $90, $76, $A2, $64, $54, $68
*WALK_3_TABLE:
* fcb	$54, $64, $90, $79, $A2, $70, $54, $6A
*WALK_4_TABLE:
* fcb	$40, $64, $90, $7C, $90, $70, $54, $6D
*WALK_5_TABLE:
* fcb	$40, $74, $90, $7F, $90, $6D, $54, $70
*WALK_6_TABLE:
* fcb	$54, $74, $90, $81, $90, $6A, $40, $70
*WALK_7_TABLE:
* fcb	$54, $70, $90, $82, $90, $68, $40, $64
****
*** Opps, a weird table
*WALK_0_TABLE:
* fcb	$54, $74, $A2, $82, $90, $66, $54, $68
*WALK_1_TABLE:
* fcb	$54, $70, $A2, $76, $90, $64, $54, $6A
*WALK_2_TABLE:
* fcb	$54, $6D, $90, $76, $A2, $64, $54, $6D
*WALK_3_TABLE:
* fcb	$54, $6A, $90, $79, $A2, $70, $54, $70
*WALK_4_TABLE:
* fcb	$54, $67, $90, $7C, $90, $70, $40, $70
*WALK_5_TABLE:
* fcb	$54, $64, $90, $7F, $90, $6D, $40, $64
*WALK_6_TABLE:
* fcb	$40, $64, $90, $81, $90, $6A, $54, $64
*WALK_7_TABLE:
* fcb	$40, $74, $90, $82, $90, $68, $54, $66
***
*   Add:  Back:  first step is farther out front (+$10), and toed in (+4)
*        Front:  last step is farther out back (+$10)
*WALK_0_TABLE:
* fcb	$58, $74, $90, $81, $A2, $64, $54, $68
*WALK_1_TABLE:
* fcb	$54, $70, $90, $82, $A2, $80, $54, $6A
*WALK_2_TABLE:
* fcb	$54, $6D, $A2, $92, $8C, $80, $54, $6D
*WALK_3_TABLE:
* fcb	$54, $6A, $A2, $76, $90, $6D, $54, $70
*WALK_4_TABLE:
* fcb	$54, $67, $8C, $76, $90, $6A, $40, $70
*WALK_5_TABLE:
* fcb	$54, $64, $90, $79, $90, $68, $40, $54
*WALK_6_TABLE:
* fcb	$40, $54, $90, $7C, $90, $66, $58, $54
*WALK_7_TABLE:
* fcb	$40, $84, $90, $7F, $90, $64, $54, $66
WALK_TABLE_END:
*========================================================================
*========================================================================
* RAM usage:
*	$00-$01	Mode variables
*
*	$DF-E1	JMP Instruction for timer 1 interrupt (In boot ROM)
* 	$E2-FF	Stack (30 bytes:  RTI uses 9 bytes)
*
*========================================================================
*    Addresses of RAM variables:
SLAVE		EQU	$02
SLAVE_DATA	EQU	$03
SERVO		EQU	$04

*========================================================================
*    Numeric Constants:
TIMER_COUNT	EQU	5000

*========================================================================
*========================================================================
*	Start
*========================================================================
Start:
	ldx	#STAND_TABLE
	ldaa	#%10010000	; Power up A/D with clock delay
	staa	$1039
*========================================================================
*	SPI initialization:
*========================================================================
	ldaa	#%00001110	; Disable all 3 slaves.
	staa	$1004
	ldaa	#%00101111
	staa	$1008		; PortD
	ldaa	#%00111000
	staa	$1009		; Data Direction for PortD

	ldaa	#%00001110	; Disable all 3 slaves.
	staa	$1004
	ldaa	#%01010100	; MASTER (Slave is #%11000100)
	staa	$1028
*========================================================================
*	end of SPI initialization:
*========================================================================
*========================================================================
	ldx	#STAND_TABLE
	bsr	DO_8_Servos
	ldx	#DONE_LINE
	ldy	#500	; wait about 5 seconds
	jsr	DelayLong

LoopStart:
	ldx	#WALK_0_TABLE
LoopHere:
	bsr	TakeStep
	ldab	#8
	abx
	cpx	#WALK_TABLE_END
	beq	LoopStart
	bra	LoopHere
TakeStep:
	bsr	DO_8_Servos
	ldy	#5	; walk real fast
	jsr	DelayLong
	rts

*========================================================================
*	Do_8_Servos:	X points to a table of 8 values, send these
*			to the 8 servos.
*========================================================================
DO_8_Servos:
	psha
	pshb
	pshx
	clrb
LOOP_B_8:
	ldaa	$00,x
	bsr	SPI_Servo_AB
	incb
	inx
	cmpb	#$08
	blt	LOOP_B_8
	pulx
	pulb
	pula
	rts

*========================================================================
	jmp	LoopHere

DONE_LINE:
 fcc	"Standing sent."
 fcb	0
*========================================================================
*	SPI_Servo_AB:  send value in A to servo B.
*========================================================================
SPI_Servo_AB:
	pshb
	psha
	cmpb	#$03
	bgt	Set_Slave_3
	ldaa	#%11111011	; Slave number 2
	bra	Set_Slave
Set_Slave_3:
	ldaa	#%11110111	; Slave number 3
Set_Slave:
	staa	SLAVE
	pula
	andb	#$03	; each slave wants servo 0-3, mask off 4-7
	jsr	SPI_Servo
	pulb
	rts
*========================================================================
DelayMedium:
	pshx
	ldx	#$1000
L_loop:	dex
	bne	L_loop
	pulx
	rts

*========================================================================
DelayLong:
	pshx
	pshy
*	ldy	#20	; 20 decimal = 3.94S
Y_LOOP:
*	ldx	#$0000	; X loop is 197 mS
	ldx	#$1000	; X loop is 12.3125 mS
X_LOOP:	dex
	bne	X_LOOP
	dey
	bne	Y_LOOP
	puly
	pulx
	rts

*========================================================================
*	Send the sequence:  $AA $55 Servo_Num DATA $81
*	Servo_Num is in B, data is in A.
SPI_Servo:
	psha
	pshb
	stab	SERVO
	ldab	#$AA
	bsr	Send_SPI
	ldab	#$55
	bsr	Send_SPI
	ldab	SERVO
	bsr	Send_SPI
	tab
	bsr	Send_SPI
	ldab	#$81
	bsr	Send_SPI
	pulb
	pula
	jsr	DelayMedium
	rts

*========================================================================
*	Slave is indicated by a bit low in RAM location SLAVE (bit 1, 2, or 3)
*	%11111101 = Slave 1, %11111011 = Slave 2, %11110111 = Slave 3.
*	The data to send is in B.
Send_SPI:
	pshx
	psha
	ldaa	SLAVE
	ldx	#$1000
	staa	$1004		; select the slave

* SPIF_Not_Ready:
* 	brclr	$29,x $80 SPIF_Not_Ready
	stab	$102A		; Send the SPI data

SPIF_Not_Ready2:
	brclr	$29,x $80 SPIF_Not_Ready2

	ldaa	#%00001110	; Disable all 3 slaves.
	staa	$1004

	ldaa	$102A		; Get the slave's data
	staa	SLAVE_DATA
	pula
	pulx
	rts
*========================================================================