;============================================================================================================
;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] --> /               \
;					   | Control Board | --> [Control Relays] --> [Drive]
;	[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
ax	 	ds	1	; temporary variable
bx	 	ds	1	; temporary variable
cx	 	ds	1	; temporary variable
command0 	ds	1	; command encoder position LB
command1 	ds	1	; command encoder position HB
feedback0   	ds	1	; feedback encoder position LB
feedback1   	ds	1	; feedback encoder position HB
testcw   	ds	1	; test variable
testccw    	ds	1	; test variable
control		ds	1	; analog control of system

;================Debug This==================================================================================
		watch	command,8,udec
		watch	feedback,8,udec

;================Start=======================================================================================
		org	0
start
		mov	!ra,#$0		; Set port A as outputs
					; ra.3 always on
					; ra.2 on if dontmove
					; ra.1 counterclockwise relay
					; ra.0 clockwise relay
		mov	!rb,#$FF	; Set port B as	inputs
		mov	!rc,#$FF	; Set port C as inputs
		mov	ra,#$0		; Turn off control relays
;================Program Go!=================================================================================
main
		

;////////////////Comparitor Algorithm////////////////////////////////////////////////////////////////////////

		mov	command,rb	; read command 
		mov	testcw,w	; copy command to testcw
		mov	feedback,rc	; read feedback
		mov	testccw,w	; copy feedback to testccw

							; Drive takes the shortest path to desired position
		sub	testcw,feedback			; if cmd - fbk < fbk-cmd then go clockwise
		sub	testccw,command 		; 
		cje	command,feedback,dontmove      	; if cmd = fbk then dont move
		cjb	testcw,testccw,clockwise	; 
							; else go counterclockwise
		and	ra,#%1010			; mask ra 
		setb	ra.1				; counterclockwise relay ON
		jmp	main				; FIN
clockwise			
		and	ra,#%1001			; mask ra
		setb	ra.0				; clockwise relay ON
		jmp	main				; FIN

dontmove			
		and	ra,#%1100			; ra
		setb	ra.2				; both relays OFF & Steady indicator on
		jmp	main				; FIN
;\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
;================End Program================================================================================= 