$nomod51
$include(89c51ac2.h)

STACK_SEG	SEGMENT DATA
	RSEG STACK_SEG
STACK:	DS 64
VAR1_SEG SEGMENT DATA
	RSEG VAR1_SEG

TIME1:		DS	2
TIME2:		DS	2
TIME3:		ds	2
T:		ds	2
TP:		DS	2

cseg at 0
	
	jmp main

cseg at 01bh			;interrupt request for Timer1

	jmp int

cseg at 002bh			;interrupt request for Timer2

	clr tf2
	setb p1.2		;give high ouput cause we're starting duty cycle
	setb tr1		;start Timer1 so he can count the pulse width
	reti

CSEG AT 0033H			;PCA interrupt request
	
	JMP PCAIRQ

main SEGMENT CODE
	RSEG MAIN

	MOV CMOD,#0		;set the PCA counter so he's using the same clock
	MOV 0E9H,#0		;our counters are using (gives 1ìs per count)
	MOV 0F9H,#0		

	MOV CCAPM0,#21H		;prep capturing of external values for positive slope
	CLR CCF0

	SETB CR
	SETB EC

	anl tmod,#0fh			;bunch of initializations
	orl tmod,#10h			;the meat is that timer1 is set for the 8%
	mov th1,#high(63935d)		;duty cycle that for the servos means neutral
	mov tl1,#low(63935d)		;(zero degrees) and timer2 is set for the full
	mov rcap2h,#high (46285d)	;period which is 19,25ms
	mov rcap2l,#low (46285d)
	setb tr2
	setb et1
	setb et2
	setb ea

	jmp $


int SEGMENT CODE
	RSEG INT

	PUSH ACC
	clr tf1			;timer1 stops himself cause otherwise he'll count
	clr p1.2		;as many widths he can fit in a period
	clr tr1			;he also gives low output, ending the pulse
	
	mov tl1,TP+1		;reload the timers with the calculated
	mov th1,TP		;values
	mov rcap2l,T+1
	mov rcap2h,T
	POP ACC
	reti

PCAIRQ SEGMENT CODE
	RSEG PCAIRQ

	PUSH PSW		;the PSW push is just in case it fixes the prob
	PUSH ACC
	mov 35h,ccap0l		;we store the capture registers to memory for the
	mov 36h,ccap0h		;calculations - normally this isnt necessary
	JB CCF0,TRIPPED		;but we tried it in case it fixed the problematic output
	jmp ENDINT

positive:	mov 0dah,#10001b	;switch register so that we wait for a negative
		MOV TIME2+1,35h		;slope next time (since we just tripped at
		MOV TIME2,36h		;positive).
		JMP ENDINT


TRIPPED:
	CLR CCF0
	mov a,0dah
	cjne a,#100001b,negative		;check what slope tripped us
	jmp positive

negative:	
	mov 0dah,#100001b	;wait for positive next time
	MOV TIME3+1,35h
	MOV TIME3,36h

    	CLR C			;the way this part works is it measures when the pulse
	MOV A,TIME2+1		;started, when it ended and when the next one started
	SUBB A,TIME1+1		;subtracts the times so that we end up with the pulse
	MOV TP+1,A		;width in microseconds and the pulse period in
	MOV A,TIME2		;microseconds
	SUBB A,TIME1	 
	MOV TP,A
		
     	CLR C			
	MOV A,TIME3+1
	SUBB A,TIME1+1
	MOV T+1,A
	MOV A,TIME3
	SUBB A,TIME1
	MOV T,A
	
	MOV TIME1+1,TIME3+1	;last time is will be used as the first time
	MOV TIME1,TIME3		;in the next calculation
				;cause the PCA counter doesnt stop counting, it
				;just trips and stores his value

CALC:	CLR C			;we subtract what we measured from FFFFh (65536)
	MOV A,#0FFH		;so we get the value we have to load the timers with
	SUBB A,T+1		;so that they overflow at the proper amount of time
	MOV T+1,A
	MOV A,#0FFH
	SUBB A,T
	MOV T,A

	CLR C
	MOV A,#0FFH
	SUBB A,TP+1
	MOV TP+1,A
	MOV A,#0FFH
	SUBB A,TP
	MOV TP,A

ENDINT:	POP ACC
	POP PSW
	RETI

end