;
; This file consists of a bit of example code followed
; by the actual servo controller.
;
; The Servo Controller runs up to eight servos on port
; P1. Note that on some chips, P1.0 and P1.1 require
; external pull-ups.
;
; The controller requires 38 bytes and two bits of memory.
; The user controls where this memory is placed by
; setting the FIRST_SERVO_VAR and FIRST_SERVO_BIT defines.
; The user must also set the timer-0 interrupt jump
; table entry to SERVO_ISR.
; The user should call INIT_SERVOS to allow the servo
; controller to initialize itself. Prior to this call,
; the user should turn off the timers and interrupts,
; as shown in the example code.
;
; After initialization, the interface to the controller
; is actually quite simple. Place a byte in the SERVO_UPDATE
; table, with the position indicating which pin of P1
; to control, then set the SERVO_UPDT_RDY bit to let
; the controller know that new positions are ready.
; Example:
;    mov	SERVO_UPDATE+2,	115
;    setb	SERVO_UPDT_RDY
;
; The values indicate the length of the PWM pulse according
; to the following equation:
;	width = 1000 + 4.35*value microseconds
; For the above value, 115, this works out to:
;	width = 1000 + 4.34*115 microseconds
; or about 1500 microseconds (1.5 milliseconds). This is
; the center position for most servos.
;
;   0 is ~1.0 ms
; 115 is ~1.5 ms
; 230 is ~2.0 ms
; ~4.35us per increment
; 

; Predefined constants for the TASM assembler.
#INCLUDE "8051EQU.INC"

; Eight servos
NUM_SERVOS	.equ	008H

; We tell the Servo Controller where to put its
; data by setting these values.
; The controller requires 38 bytes
; and two bits
FIRST_SERVO_VAR	.equ	048H
FIRST_SERVO_BIT	.equ	070H

.ORG   0H                    ;locate routine at 00H
    AJMP   TEST_SERVO        ;jump to START 

                             ; Set Interrupts
.ORG   03H                   ;external interrupt 0
    RETI

; The ServoController requires timer 0
.ORG   0BH                   ;timer 0 interrupt
    AJMP	SERVO_ISR

.ORG   13H                   ;external interrupt 1
    RETI

.ORG   1BH                   ;timer 1 interrupt
    RETI

.ORG   23H                   ;serial port interrupt
    RETI

.ORG   2BH                   ;timer 2  interrupt
    RETI

.ORG   33H                   ;locate beginning of rest of program

;------------------------------------------------------------------
; Start of Example 
;------------------------------------------------------------------
TEST_SERVO:

    ; P1 is the servo port
    mov		P1,		#0FFH

    ; Clear the timer control, timer mode,
    ; processor status, and interrupt enables.
    mov		TCON,		#00H
    mov		TMOD,		#00H
    mov		PSW,		#00H             
    mov		IE,		#00H               ;disable interrupts

    ; Servo Controller will set timer 0 mode to 1, meaning 16-bit
    ; It will also set the interrupt mask for timer zero interrupt,
    ; and start timer 0
    acall	INIT_SERVOS
    setb	EA

    mov		R3,		#020H
    mov		SERVO_UPDATE+2,	#0A0H
    setb	SERVO_UPDT_RDY
    mov		R2,		#010H

TESTTOP:
    ; Time Countdown... 0100H * 0100H * 010H
    djnz	R0,		TESTTOP
    djnz	R1,		TESTTOP
    djnz	R2,		TESTTOP
    ; Timer Reset
    mov		R2,		#010H

    ; R3 starts out as 020H
    ; This bounces it back and forth between 020H and 0A0H
    mov		A,		#080H
    xrl		A,		R3
    mov		R3,		A

    ; Set the position for servo channel 2 (out of 0..7)
    mov		SERVO_UPDATE+2,	A
    setb	SERVO_UPDT_RDY
    ajmp	TESTTOP


;------------------------------------------------------------------
; Start of Servo Controller
;------------------------------------------------------------------


;**************************************************************************
;
; Serial Port Initialization
; Call this to start the serial port
;

;
; Servo Defines:
;	In:
;		NUM_SERVOS	Number of servos being served
;		FIRST_SERVO_VAR	Memory position of the first servo variable
;		FIRST_SERVO_BIT	Memory position of the first servo bit variable
;	Out:	
;		AFTER_SERVO_VAR	Memory position after last servo variable
;		AFTER_SERVO_BIT	Memory position after last servo bit variable
;
;	
;
; Servo Variables:
;	SERVO_TABLE	3*(NUM_SERVOS+1) bytes, each layed out as:
;		TH	Hi byte of time to keep current state
;		TL	Lo byte of time to keep current state
;		STATE	Current State
;
;	SERVO_INDEX	1  byte, countdown counter for main servo loop
;	SERVO_COUNT	1  byte, number of entries in servo table (varies due to pruning)
;	SERVO_TBL_PTR	pointer to current record in servo table
;
;	SERVO_UPDATE	NUM_SERVOS bytes, position of the servo 0-230
;
;
;	SERVO_UPDT_RDY	Bit indicating that SERVO_UPDATE has new values
;
R0_mem		.equ	0
R1_mem		.equ	1
R2_mem		.equ	2
R3_mem		.equ	3
R4_mem		.equ	4
R5_mem		.equ	5
R6_mem		.equ	6
R7_mem		.equ	8

SERVO_TABLE	.equ	FIRST_SERVO_VAR				; 27
SERVO_INDEX	.equ	SERVO_TABLE + (3*(NUM_SERVOS+1))	;  1
SERVO_COUNT	.equ	SERVO_INDEX + 1				;  1
SERVO_TBL_PTR	.equ	SERVO_COUNT + 1				;  1
SERVO_UPDATE	.equ	SERVO_TBL_PTR + 1			;  8
								; 38 total
SERVO_SZ	.equ	38

SERVO_UPDT_RDY	.equ	FIRST_SERVO_BIT
AFTER_SERVO_BIT	.equ	FIRST_SERVO_BIT+1

SERVO_BIT_SZ	.equ	2

INIT_SERVOS:
	
    mov		P1,		#00

    mov		SERVO_UPDATE+0,	#00	; turned off at 1111
    mov		SERVO_UPDATE+1,	#00	; turned off at  941
    mov		SERVO_UPDATE+2,	#00	; turned off at 1141
    mov		SERVO_UPDATE+3,	#00	; turned off at  938
    mov		SERVO_UPDATE+4,	#00	; turned off at	1010
    mov		SERVO_UPDATE+5,	#00	; turned off at 1036
    mov		SERVO_UPDATE+6,	#00	; turned off at 1121
    mov		SERVO_UPDATE+7,	#00	; turned off at  951

    acall	UPDATE_SERVO

    MOV		SERVO_TBL_PTR,	#SERVO_TABLE
    MOV		SERVO_INDEX,	SERVO_COUNT
    INC		SERVO_INDEX

    ;
    ; start the timer
    ;
    mov		TH0,		#001H
    mov		TL0,		#000H
    orl		TMOD,		#001H
    setb	ET0
    setb	TR0
    setb	PT0

    ret

SERVO_ISR:
    ; save state
    PUSH	PSW				; 2
    PUSH	0 ; R0				; 2

    ;
    ; move current parameters and length of time to hold them
    ; to P1, timer, respectively
    ;
    MOV		R0,		SERVO_TBL_PTR	; 2
    MOV		TH0,		@R0		; 2
    INC		R0				; 1
    MOV		TL0,		@R0		; 2
    INC		R0				; 1
    MOV		P1,		@R0		; 2
    INC		R0				; 1
    MOV		SERVO_TBL_PTR,	R0		; 2
    DJNZ	SERVO_INDEX,	SERVO_DONE	; 2

    ;
    ; We are at the end of the loop. Reload.
    ;
    jnb		SERVO_UPDT_RDY,	NO_UPDATE

    clr		SERVO_UPDT_RDY

    ;
    ; Push vars used by SERVO_UPDATE
    ;
    push	ACC
    push	1
    push	2
    push	3
    push	4
    push	5
    push	6
    acall	UPDATE_SERVO
    pop		6
    pop		5
    pop		4
    pop		3
    pop		2
    pop		1
    pop		ACC

NO_UPDATE:

    ;
    ; No update ready; simply reset the
    ; servo table
    ;

    MOV		SERVO_TBL_PTR,	#SERVO_TABLE
    MOV		SERVO_INDEX,	SERVO_COUNT
    INC		SERVO_INDEX

SERVO_DONE:
    pop		0
    pop		PSW
    reti

;
; Update:	nServos bytes:	angle (0-250)
;
;	First set timer transition times
;
;		1 second = 11,059,000 clock cycles, = 921583.333 machine cycles
;		1 millisecond = 11,059 cycles = 921.583333 machine cycles
;
;		921 == 0x0399
;
;	Transition time is 1 ms plus angle*4
;		mov	lo,	099H
;		mov	hi,	003H
;	timer-hi timer-lo P1-state
;

UPDATE_SERVO:
;
; Step 1: Copy
;
    mov		R0,		#SERVO_TABLE
    mov		R1,		#SERVO_UPDATE
    mov		R2,		#001H
    mov		SERVO_INDEX,	#NUM_SERVOS
COPY_LOOP:
    mov		A,		@R1		; load new value into A
    mov		@R0,		A		; mov new value into TH
    inc		R1				; reference next new value
    inc		R0				; skip TL (we will get later)
    inc		R0				; reference mask
    mov		A,		R2		; move servo number into A
    mov		@R0,		A		; move servo number into table
    rl		A				; reference next servo
    mov		R2,		A		; store servo mask
    inc		R0				; reference next row in table

    djnz	SERVO_INDEX,	COPY_LOOP

;
; Step 2: Sort (bubble sort: deterministic timing)
;

    mov		A,		#NUM_SERVOS	; inner loop counter
    dec		A
    jz		NO_NEED_TO_SORT

    mov		R2,		A		; R2 is the outer loop counter
    mov		R4,		A		; R4 holds the inner loop reload

SORT_OUTER_LOOP:
    mov		R3,		R4_mem 		; R3 is the inner loop counter
    mov		R0,		#SERVO_TABLE	; reload lo pointer
    mov		R1,		#SERVO_TABLE	; reload hi pointer
    inc		R1
    inc		R1
    inc		R1

SORT_INNER_LOOP:
    ; compare current time to next time, swap if necessary
    ; compare hi
    clr		C
    mov		A,		@R1		
    subb	A,		@R0		; subtract lo from hi
    jc		DO_SWAP				; if lo > hi, carry will be set

NO_SWAP:
    inc		R0
    inc		R0
    inc		R0
    inc		R1
    inc		R1
    inc		R1
    ajmp	AFTER_SWAP

DO_SWAP:
    ; Transfer Hi
    mov		R5_mem,		@R0
    mov		R6_mem,		@R1
    mov		@R0,		R6_mem
    mov		@R1,		R5_mem
    inc		R0
    inc		R1

    ; Transfer Lo ... no need: no info in low yet.
    ;mov		R5_mem,		@R0		; R5
    ;mov		R6_mem,		@R1		; R6
    ;mov		@R0,		R6_mem
    ;mov		@R1,		R5_mem
    inc		R0
    inc		R1

    ; Transfer mask
    mov		R5_mem,		@R0		; R5
    mov		R6_mem,		@R1		; R6
    mov		@R0,		R6_mem
    mov		@R1,		R5_mem
    inc		R0
    inc		R1

AFTER_SWAP:
    djnz	R3,	SORT_INNER_LOOP
    djnz	R2,	SORT_OUTER_LOOP

NO_NEED_TO_SORT:

;
; Step 3: Prune
;
    mov		SERVO_INDEX,	#NUM_SERVOS
    mov		SERVO_COUNT,	#NUM_SERVOS
    djnz	SERVO_INDEX,	DO_PRUNE
    ajmp	AFTER_PRUNE
DO_PRUNE:
    mov		R0,		#SERVO_TABLE	; lo servo
    mov		R1,		#SERVO_TABLE	; hi servo
    inc		R1
    inc		R1
    inc		R1

PRUNE_TOP:
    ; compare (@R1 guaranteed >= @R0)
    clr		C
    mov		A,		@R1
    subb	A,		@R0
    ; if A <= 4, combine. Carry will be clear already.
    subb	A,		#5
    jnc		NO_COMBINE

COMBINE:
    dec		SERVO_COUNT
    ; reference mask
    inc		R0
    inc		R0
    inc		R1
    inc		R1
    ; combine masks into lo
    mov		A,		@R1
    orl		A,		@R0
    mov		@R0,		A
    ; move lo-ptr back to reference same,
    ; move hi-ptr forward to next
    dec		R0
    dec		R0
    inc		R1

    ; R0 references the same table entry,
    ; R1 is advanced to next entry
    djnz	SERVO_INDEX,	PRUNE_TOP
    sjmp	AFTER_PRUNE

NO_COMBINE:

    ; advance R0 to the next entry
    inc		R0
    inc		R0
    inc		R0

    ;
    ; Propagate. If no pruning has happened,
    ; this will copy values onto themselves,
    ; but other than wasting time, there is
    ; no ill effect (plus it makes the code
    ; simpler than checking/branching on equality).
    ;
    mov		A,		@R1
    mov		@R0,		A
    inc		R0
    inc		R1

    ; no need... no info in TL yet
    ;mov		A,		@R1
    ;mov		@R0,		A
    inc		R0
    inc		R1

    mov		A,		@R1
    mov		@R0,		A
    inc		R1

    dec		R0
    dec		R0

    djnz	SERVO_INDEX,	PRUNE_TOP
AFTER_PRUNE:


; Step 4: adjust
;


    ;
    ; Try 2
    ;
    ;		R0 - index register
    ;		R1 - accumulating mask
    ;
    ;		R2,R3	- arithmetic register
    ;		R4	- last time
    ;		R5 	- accumulating adjust
    ;		R6 	- loop index
    ;
    mov		R0,		#SERVO_TABLE
    mov		R1,		#0FFH
    mov		R4,		#0
    mov		R5,		#0
    mov		R6,		SERVO_COUNT

ADJ_LOOP_TOP
    ; load absolute time and convert to time delta by subtracting
    ; prior time. Store unmodified absolute time for next loop.
    mov		A,		@R0
    clr		C
    subb	A,		R4
    mov		R4_mem,		@R0

    ;
    ; We now have the time delta. We need to multiply by eight,
    ; then negate (we want to wait 'time delta'; the timer counts
    ; up, so we subtract time-delta from 0x10000, which is equivalent
    ; to invert). We use "flip bits and add one". We also need to
    ; add an accumulating adjustment (makes up for the time spent
    ; in all ISRs called before, 15 clocks per). We combine the
    ; "add one" and "subtract 15". Because the subtract is from
    ; the time not the inverted time, the subtract becomes an
    ; add. We simply increment the accumulating adjust (R5) for the
    ; '+1', add it, then decrement the '+1' back out by adding
    ; 14 rather than 15.
    ;
    rl		A
    rl		A
    rl		A
    xrl		A,		#0FFH
    mov		R2,		A

    inc		R5
    clr		C
    ; TL
    orl		A,		#007H
    ;addc	A,		R5
    addc	A,		#14
    mov		R3,		A

    ; TH
    mov		A,		R2
    orl		A,		#0F8H
    addc	A,		#0
    mov		@R0,		A
    inc		R0
    mov		@R0,		R3_mem
    inc		R0

    ; add accumulating error (minus 1 for the increment done above)
    ;mov		A,		#14
    ;add		A,		R5
    ;mov		R5,		A
    ;dec R5

    ; set the mask
    mov		A,		@R0
    mov		@R0,		R1_mem
    xrl		A,		#0FFH
    anl		A,		R1
    mov		R1,		A

    inc		R0

    djnz	R6,		ADJ_LOOP_TOP

    ; Now adjust the first delta to add in the fixed 1ms
    clr		C
    mov		A,		SERVO_TABLE+1
    ;subb	A,		#09AH
    subb	A,		#080H
    mov		SERVO_TABLE+1,	A
    mov		A,		SERVO_TABLE
    subb	A,		#002H
    mov		SERVO_TABLE,	A

    ; Finally, add the trailing dwell
    mov		@R0,		#0A9H
    inc		R0
    mov		@R0,		#09AH
    inc		R0
    mov		@R0,		#000H

    MOV		SERVO_TBL_PTR,	#SERVO_TABLE
    MOV		SERVO_INDEX,	SERVO_COUNT
    INC		SERVO_INDEX
    ret

.END