;*********************************************************
;    File:		
;    Autor:		M. Lüdin & M. Birchmeier
;    Zweck:		
;*********************************************************
;   

;*********************************************************

$mod51		;SFR Deklarationen einlesen

;*** Variabeln *******************************************
SFLAG	DATA	30H
SERVO1	DATA	31H	;nicht ändern, pgm bezieht sich drauf
SERVO2	DATA	32H
SERVO3	DATA	33H
SERVO4	DATA	34H

disth	DATA	37H    
distl	DATA	38H
flag	data	39h
;*********************************************************


;*** PIN Belegung **************************************
SPIN1	set	P1.7
SPIN2	set	P1.6
SPIN3	set	P1.5
SPIN4	set	P1.4

ultra	set	P3.0
ultrb	set	p3.1
noresp	set	p1.3
;*********************************************************


;*** Konstanten *****************************************

;     Gelten nur für 1Mhz Systemtakt (ca 12MHz Quarz)
CSERVO	EQU	004H	;Anzahl Servos

T0LOW 	EQU	010H
T0HIGH	EQU	027H

MSOL	EQU	000H	;offset pulswidth
MSOH	EQU	003H
range	equ	005h	;pw range

brstc	EQU	0eH	


;***************************************************************


;*** Reset Code ************************************************
	CSEG
	org 	0		;reset vector
	ajmp	Reset

	org	03h		;ExtInterrupt0
	ajmp	ISRext0

	org	0BH		;T0 Vector
	ajmp	ISRtimer0

	org	13h		;ExtInterrupt1
	ajmp	ISRext1

	org	1BH		;T1 Vector
	ajmp	ISRtimer1

;*** Startup Code **********************************************
Reset:
	mov	sp,#6fH		;Stack: 70..7f
	mov	TMOD,#00010001B	;T0,T1: Mode 1 (16 Bit)

	mov	TH0,#T0HIGH
	mov	TL0,#T0LOW

	setb	TR0		;enable Timer0
	setb	ET0		;enable timer0 interrrupt
	setb	EA		;enable interrupts

	setb	tr1		;enable timer1
	setb	et1		;enable timer1 interrupt
	setb	ex0		;------ ext interrupt0
	setb	it0		;interrupt0 flankengesteuert

	MOV	SFLAG,#0
	MOV	SERVO1,#080H
	MOV	SERVO2,#07fH
	MOV	SERVO3,#000H
	MOV	SERVO4,#0FFH


	setb	ultra
	clr	ultrb

;*** Main  *******************************************
 
Main:
       call   sendus
twait:
	mov	a,th1
	jz	timup
	jnb	p3.6,twait    

	clr	noresp
	mov	servo1,th1

timup:
	mov	a,tl1
	jnz	twait
rwait:
	jnb	noresp,rwait

	jmp	Main


;--------------------------------------------------------------------------
sendus:
	mov	tl1,#00h	
	mov	th1,#00h
	setb	tr1		;timer1 neustarten

	mov	a,#brstc		;schaltzahl pro burst
burst:
	dec	a
	nop
	nop
	nop			;verzögerung
	nop
	nop
	nop
	cpl	ultra
	cpl	ultrb
	jnz	burst
       ret

;*** Timer 0 ***************************************************
;  Wird zu Steuerung der Servos verwendet
ISRtimer0:
	push	ACC
	push	b
	PUSH	PSW

	CLR 	TR0		;Timer0 anhalten
	MOV	A,SFLAG
	JNZ	PW

PERIODE:
;---------------------------------------------------------------------

;---------------------------------------------------------------------
	CLR	SPIN1		;Ausgänge auf 0 setzen
	CLR	SPIN2		
	CLR	SPIN3		
	CLR	SPIN4		
	MOV	SFLAG,#CSERVO	;Anzahl Servos laden
	MOV	TH0,#-T0HIGH
	MOV	TL0,#-T0LOW	
	JMP	T0END	

PW:
	CLR	SPIN1		;Ausgänge auf 0 setzten
	CLR	SPIN2		
	CLR	SPIN3		
	CLR	SPIN4		

	MOV	A,SFLAG		;Servo nummer nach A laden
	DEC	A
	JZ	PWPIN1
	DEC	A
	JZ	PWPIN2
	DEC	A
	JZ	PWPIN3

PWPIN4:	SETB	SPIN4		;Entsprechender Ausgang auf 1 setzten
	JMP	PWLOAD

PWPIN3:	SETB	SPIN3
	JMP	PWLOAD

PWPIN2:	SETB	SPIN2
	JMP	PWLOAD

PWPIN1:	SETB	SPIN1
	
PWLOAD:
	MOV	A,#30H		;Basisadresse laden
	ADD	A,SFLAG		;Zeiger bilden (für die variablen SERVO1-4)
	MOV	R0,A
	MOV	A,@R0		;Servo Stellung nach A Laden

	MOV	B,#range
	MUL	AB
	ADD	A,#MSOL
	CPL	A		
	MOV 	TL0,A
	MOV	A,B
	ADDC	A,#MSOH
	CPL	A		
	MOV	TH0,A
	DEC	SFLAG		;nächster Servo
T0END:
	SETB	TR0		;Timer0 nach programmierung starten

	pop	PSW
	pop	b
	pop	ACC
reti

;*** EXTinterrupt0 ********()*********************

ISRext0:
	push	ACC
	PUSH	PSW	
	
	pop	PSW
	pop	ACC
reti


;*** timer1 ***************(ultraschall)********************

ISRtimer1:
	push	ACC
	PUSH	PSW

	setb	noresp

	clr	tr1		
	mov	tl1,#00h	
	mov	th1,#00h      ;timer1 stop/reset



	pop	PSW
	pop	ACC
reti



;*** EXTinterrupt1 ********()*********************

ISRext1:
	push	ACC
	PUSH	PSW	
	
	pop	PSW
	pop	ACC
reti






;*** Programm Ende ***
end