;********************************************************************
;
; Author        : Michael Müller-Aulmann
;
; Date          : 20. September 2001
;
; File          : Servo.asm
;
; Hardware      : Any 8052 based MicroConverter (ADuC812)
;
; Description   : Controls 8 Servos at Port P2
;
;		  For questions - please contact me by EMail :
;		
;			Michael.Mueller-Aulmann@analog.com
;
;********************************************************************

$MOD812                          ; use ADuC812 predefined symbols
;____________________________________________________________________
; CONSTANTS
; Servo Pulse Times @ 11,0592MHz

LEFT		EQU	0FCC2h	; = 0,9 ms
CENTER		EQU	0FA99h	; = 1,5 ms
RIGHT		EQU	0F870h	; = 2,1 ms


DSEG
;____________________________________________________________________
; VARIABLES
ORG		020h
; Servo Pulse Times for 8 Servos
SERVO0:	DS	2
SERVO1:	DS	2
SERVO2:	DS	2
SERVO3:	DS	2
SERVO4:	DS	2
SERVO5:	DS	2
SERVO6:	DS	2
SERVO7:	DS	2

State:	DS	1	; Timer 2 State
Delay:	DS	2	; Delay Counter - countdown in Timer 2 ISR every 2,5ms
STACK:	DS	8	; Stack
;********************************************************************
; MACROS
WAIT	MACRO	DelayTime	; multiples of 2,5 ms
	mov	Delay,#High(DelayTime)
	mov	(Delay+1),#Low(DelayTime)
	Call	WaitDelay
ENDM

SERVO	MACRO	ServoNr, Position
	mov	r0,#(SERVO0 + (ServoNr * 2))
	mov	@r0,#HIGH(Position)
	inc	r0
	mov	@r0,#LOW(Position)
ENDM

CSEG
;********************************************************************
; INTERRUPTS
ORG	0000h
RESET:
	jmp	MAIN
;____________________________________________________________________
ORG	000Bh
;Servo Puls
Timer_0_Interrupt:
	mov	P2,#0	; Clear all Servo Outputs
	clr	TR0	; Stop Timer 0
	reti
;____________________________________________________________________
ORG	002BH
; Servo Cycle
Timer_2_Interrupt:
	push	acc			; Save Context
	push	psw
	push	dpl
	push	dph
	mov	a,State			; Get Cycle State
	add	a,State			; * 3
	add	a,State
	inc   	State			; Next State
	mov	dptr,#StateTable
	jmp	@a+dptr			; Jump to actual Servo Cycle
StateTable:
	ljmp	Servo_0
	ljmp	Servo_1
	ljmp	Servo_2
	ljmp	Servo_3
	ljmp	Servo_4
	ljmp	Servo_5
	ljmp	Servo_6
	ljmp	Servo_7

; Get Servo Pulse Width and set Pulse Output
Servo_0:
	setb	P2.0
	mov	TL0,(SERVO0 + 1)
	mov	TH0,SERVO0
	jmp	EndCycle
	
Servo_1:
	setb	P2.1
	mov	TL0,(SERVO1 + 1)
	mov	TH0,SERVO1
	jmp	EndCycle
	
Servo_2:
	setb	P2.2
	mov	TL0,(SERVO2 + 1)
	mov	TH0,SERVO2
	jmp	EndCycle
	
Servo_3:
	setb	P2.3
	mov	TL0,(SERVO3 + 1)
	mov	TH0,SERVO3
	jmp	EndCycle
		
Servo_4:
	setb	P2.4
	mov	TL0,(SERVO4 + 1)
	mov	TH0,SERVO4
	jmp	EndCycle
		
Servo_5:
	setb	P2.5
	mov	TL0,(SERVO5 + 1)
	mov	TH0,SERVO5
	jmp	EndCycle
	
Servo_6:
	setb	P2.6
	mov	TL0,(SERVO6 + 1)
	mov	TH0,SERVO6
	jmp	EndCycle
		
Servo_7:
	setb	P2.7
	mov	TL0,(SERVO7 + 1)
	mov	TH0,SERVO7
	mov	State,#0		; Restart with State 0
	
EndCycle:
; Start Servo Pulse Timer0

	mov	a,TMOD			; Get Timer Mode
	orl	a,#001H			; Add Timer 0 16-Bit Timer Mode
	mov	TMOD,a			; Set Timer Mode
	setb	TR0			; Start Timer 0

; Generate Delay
	mov	dph,Delay		; Delay == 0 ?
	mov	dpl,(Delay + 1)
	mov	a,dpl
	orl	a,dph
	cjne	a,#0,Decrement		; No 	-> Decrement
	jmp	NoDecrement		; Yes 	-> No Decrement needed
Decrement:		
	mov	a,dpl			; Decrement Low Byte
	dec	dpl
	cjne	a,#0,NoHighDec		; Low Byte == 0 ?
	dec	dph			; Yes 	-> Decrement High Byte
	mov	Delay,dph		
NoHighDec:
	mov	(Delay + 1),dpl		; Store actual Delay Count
NoDecrement:		
	clr	TF2			; Clear Timer 2 Interrupt Flag
	pop	dph			; Restore Context
	pop	dpl
	pop	psw
	pop	acc
	reti				; End Interrupt

;====================================================================
; MAIN PROGRAM
;====================================================================
MAIN:
	mov	sp,#STACK		; Init Stackpointer

; Init Timer 2 Interrupt with Autoreload 2,5 ms @ 11,0592MHz
	mov	RCAP2H,#HIGH(0FFFFH - 2304)
	mov	TH2,#HIGH(0FFFFH - 2304)
	mov	RCAP2L,#LOW(0FFFFH - 2304)
	mov	TL2,#LOW(0FFFFH - 2304)
	mov	T2CON,#004H		; Start Autoreload
      	mov	State,#0		; State in 20 ms Cycle

; Enable Timer 0/2 Interrupt
	mov	IE,#0A2H		

; High Priority for Timer 0/2 Interrupt
	mov	IP,#022H
;____________________________________________________________________
; Init Servo Default Values "CENTER"
	mov	P2,#0			; Clear all P2 Pins
	call	SETCENTER		; all Servos to CENTER
LOOP:
	WAIT	40			; wait 100 ms
	call	SETLEFT			; all Servos to LEFT Position
	WAIT	40			; wait 100 ms
	call	SETRIGHT		; all Servos to RIGHT Position
	WAIT	40			; wait 100 ms
	SERVO	0,LEFT			; Set Servo 0 to LEFT
	SERVO	1,CENTER		; Set Servo 1 to CENTER
	SERVO	2,LEFT			; Set Servo 2 to LEFT
	SERVO	3,CENTER		; Set Servo 3 to CENTER
	WAIT	20			; wait 50 ms
	SERVO	4,LEFT			; Set Servo 4 to LEFT
	SERVO	5,CENTER		; Set Servo 5 to CENTER
	SERVO	6,LEFT			; Set Servo 6 to LEFT
	SERVO	7,CENTER		; Set Servo 7 to CENTER
	WAIT	80			; wait 200ms
	call	SETRIGHT		; all Servos to RIGHT Position
	WAIT	400			; wait 400 * 2,5ms = 1000ms = 1s
	call	SETCENTER		; all Servos to CENTER Position
	WAIT	200			; wait 0,5s
	call	SETRIGHT		; all Servos to RIGHT Position
	jmp	LOOP			; Loop forever

;====================================================================
; SUBROUTINES
;====================================================================
SETCENTER:
; Set all Servo Pulse Times for CENTER Position
	mov	r2,#8			; Set Loop Count
	mov	r0,#SERVO0		; Start Index
CenterLoop:
	mov	@r0,#HIGH(CENTER)	; Set High Byte
	inc	r0			; Next Index
	mov	@r0,#LOW(CENTER)	; Set Low Byte	
	inc	r0			; Next Index
	djnz	r2,CenterLoop		; Dec Loop Count until 0
	ret				; Return to Caller
;____________________________________________________________________
SETLEFT:	
; Set all Servo Pulse Times for LEFT Position
	mov	r2,#8			; Set Loop Count
	mov	r0,#SERVO0		; Start Index
LeftLoop:
	mov	@r0,#HIGH(LEFT)		; Set High Byte
	inc	r0			; Next Index
	mov	@r0,#LOW(LEFT)		; Set Low Byte	
	inc	r0			; Next Index
	djnz	r2,LeftLoop		; Dec Loop Count until 0
	ret				; Return to Caller
;____________________________________________________________________
SETRIGHT:
; Set all Servo Pulse Times for RIGHT Position
	mov	r2,#8			; Set Loop Count
	mov	r0,#SERVO0		; Start Index
RightLoop:
	mov	@r0,#HIGH(RIGHT)	; Set High Byte
	inc	r0			; Next Index
	mov	@r0,#LOW(RIGHT)		; Set Low Byte	
	inc	r0			; Next Index
	djnz	r2,RightLoop		; Dec Loop Count until 0
	ret				; Return to Caller
;____________________________________________________________________
WaitDelay:
; Wait until Delay is counted down by Timer 2 Interrupt
	mov	a,Delay			; Get Delay
	orl	a,(Delay+1)
	jnz	WaitDelay		; Delay == 0 ? No -> Test again
	ret				; Yes -> Return
;====================================================================
	END