;============================================================================================================
;Absolue Position Encoder Comparitor
;============================================================================================================

	;-------------------------------------------------------;			  
	;   File...... 	APEC256.SRC				;	
	;   Purpose... 	Control a directional drive's position 	;
	;   Author.... 	Nick Bernard 				;
	;   E-mail.... 	nickemtelect@sw.rr.com 			;
	;   Started... 	02.16.04				;
	;   Version...	1					;
	;-------------------------------------------------------;

;================System in General===========================================================================
;					    _______________
;	[Pilot]  --> [Command Encoder] --> /               \
;					   |    CTI-PEC    | --> [Control Relays] --> [Drive]
;					   | Control Board |						
;	[Drive]  --> [Feeback Encoder] --> \_______________/
;
;
;	Pilot..........	Turns a nob to desired drive direction (direction of thrust)
;	drive..........	A large thruster capable of 360 degrees of rotation
;	Encoder........	Both encoders are ED-21-P (9 bit Binary Parallel) magnetic position encoders
;	CTI-PEC........	The circuit board that houses this SX mcu & other interface hardware	
;	Control Relays.	DC relays that ultamately control drive rotation
;	
;
;============================================================================================================

;================Device======================================================================================
		device	sx28l,oschs3
		device	turbo, stackx, optionx
		IRC_CAL	IRC_SLOW
		reset	start
		freq	50_000_000 		;50MHz

;================Variables===================================================================================
		org	8
command	 	ds	1	; command encoder position
feedback   	ds	1	; feedback encoder position
testcw   	ds	1	; test variable
testccw    	ds	1	; test variable
prec_mask 	ds	1	; precision mask determines drive position precision

D1		equ	$FF	; 1.41 degrees
D3		equ	$FE	; 2.8125 degrees
D6		equ	$FC	; 5.625 degrees

;================Debug This==================================================================================
		watch	command,8,udec
		watch	feedback,8,udec

;================Start=======================================================================================
		org	0
start
		mov	!ra,#$0		; Set port A as outputs
		mov	!rb,#$FF	; Set port B as	inputs
		mov	!rc,#$FF	; Set port C as inputs
		mov	ra,#$0		; Turn off control relays
		mov	prec_mask,#D1  ; Set precision mask

;================Program Go!=================================================================================
main
		or	ra,#%1000
		mov	command,rb	; read command 
		mov	testcw,w	; copy command to testcw
		mov	feedback,rc	; read feedback
		mov	testccw,w	; copy feedback to testccw
		
;		CJA	prec_mask,#D1,no_pmask
;		and	feedback,prec_mask	; reduce precision
;		and	command,prec_mask	; ^	
;		and	testcw,prec_mask	; ^	
;		and	testccw,prec_mask	; ^	
;no_pmask

;////////////////Comparitor Algorithm////////////////////////////////////////////////////////////////////////
							; Drive takes the shortest path to desired position
		CJE	command,feedback,dontmove      	; ? cmd = fbk -> {disable...}	
		sub	testcw,feedback			; ? cmd - fbk < fbk-cmd -> {clockwise...}
		sub	testccw,command 		; ^
		CJB	testcw,testccw,clockwise	; ^
		mov	ra,#%0010			; ~ counterclockwise relay ON
		jmp	main				; FIN
clockwise	mov	ra,#%0001			; ~ clockwise relay ON
		jmp	main				; FIN
dontmove	mov	ra,#%0100			; ~ both relays OFF & Steady indicator on
		jmp	main				; FIN
;\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\

 