* THIS CODE WAS POSTED TO THE PUBLIC DOMAIN
* SOURCE UNKNOWN

* SERVO MOTOR DEFINITIONS
* SERVO CONSTANTS
* ADJUST THESE TO GET YOUR SERVO TO GET FULL TRAVEL IN $00-$FF SETTING RANGE

*MINTIME	        EQU     200             ;MIN HIGH TIME=100uS, (100uS/.5uS)=200 CYCLES	=-90 DEGREES
MINTIME	        EQU     1700             ;MIN HIGH TIME=100uS, (100uS/.5uS)=200 CYCLES	=-90 DEGREES
MAXTIME		EQU	4600		;MAX HIGH TIME=2300uS (2300uS/.5uS)=4600 CYCLES =+90 DEGREES
SERVMUL 	EQU    	12		;MULTIPLIER TO MAKE $00-$FF COVER FULL SERVO RANGE (4400 CYCLES)

XSERV_POS	RMB	1		;X-AXIS SERVO MOTOR POSITION (0 -> 255) = (0 -> 100%)
YSERV_POS	RMB	1		;Y-AXIS SERVO MOTOR POSITION (0 -> 255) = (0 -> 100%)


*	WHAT WE WANT TO DO IS TO GET PWM SINALS GOING FOR THE SERVO AND DRIVETRAIN MOTORS. THE FREE
* RUNNING COUNTER WILL CYCLE AT .5uS *2^16 = 32.768 mS, WHICH WILL WORK NICELY.  WE WANT A RANGE FROM 1ms
* TO 2mS HIGH, AND THE REST LOW. COMPARING THE COUNTER VALUES, STARTING AT $0000, 1mS LATER WE HAVE
* $07D0, 1.5 AT $0BB8, AND 2 AT $0FA0 (DECIMAL 2000,3000, AND 4000). IF WE SET SERVO OUTPUTS HIGH
* AT $0000 OF THE FREE RUNNING COUNTER, WE CAN USE THE 4 TIMER COMPARE REGISTERS TO CONTROL BITS
* 3,4,5,&6 ON PORT A. USE THE DESIRED VALUE IN THE TIMER COMPARE AND CONFIGURE THE COMPARE TO TOGGLE
* THE PORT BIT. THIS WILL HAPPEN AUTOMATICALLY; THE PORT BITS ARE SET HIGH WHEN THE FREE RUNNING
* COUNTER ROLLS OVER, USING OC1. NO INTERRUPTS ARE NEEDED.
*
*
*		LDAA    #%10001011      ;PA7 IS OUTPUT,PA OFF,X,PA3 IS OUTPUT,PA3 IS 0C5,X,32MS FOR RTI
*               STAA    PACTL

*		BSET TFLG2,X %01000000	;CLEAR PENDING RTIF INTERRUPT FLAG
*		BSET TMSK2,X %01000000	;TURN ON REAL TIME INTERRUPTS		

		BSET OC1M,X  %01111000  ;BITS PA6,PA5,PA4,PA3 ARE AFFECTED BY OC1
		BSET OC1D,X  %01111000  ;1'S ARE OUTPUT ON SUCCESFUL COMPARE
		CLR	TOC1		;SET 0C1 TO SET BITS PA3,PA4,PA5,PA6 WHEN TCNT=0
		LDAA  	#%10101010  	;SET 0C2,OC3,OC4,0C5 TO RESET LOW ON COMPARE WITH TCNT
		STAA    TCTL1
		CLR	TOC2		;SET OC2 TO CLEAR BIT PA6 WHEN TCNT=0 (0% DUTY CYCLE)
		CLR	TOC3		;SET OC3 TO CLEAR BIT PA5 WHEN TCNT=0 (0% DUTY CYCLE)
		CLR	TOC4		;SET OC4 TO CLEAR BIT PA4 WHEN TCNT=0 (0% DUTY CYCLE)
		CLR	TOC5		;SET OC5 TO CLEAR BIT PA3 WHEN TCNT=0 (0% DUTY CYCLE)
		                

SERVO_INIT      LDAA    #$7F      	 ;SET SERVOS TO CENTRAL POSITION
		JSR	SERVO_SET_X
		LDAA    #$7F
		JSR	SERVO_SET_Y
		

SERVO_SET_X:	STAA	XSERV_POS	;STORE POSITION IN MEMORY SO IT CAN BE REFFERED TO LATER
		LDAB 	#SERVMUL	;First convert settings to motor-usable values
                MUL			;MULTIPLY BY SERVMUL
                ADDD	#MINTIME	;ADD THE MININUM HIGH DUTY CYCLE TIME
		STD	TOC5	        ;store to servo 1 memory
		RTS

SERVO_SET_Y:	STAA	YSERV_POS
		LDAB 	#SERVMUL	;First convert settings to motor-usable values
                MUL			;multiply by SERVMUL
                ADDD	#MINTIME	;ADD THE MININUM HIGH DUTY CYCLE TIME
		STD	TOC4	        ;store to servo 1 memory
		RTS