; Test program for servos

PWEN:   equ $0042       ; PWM enable register
PWPER0: equ $004C       ; PWM period channel for channel 0
PWPER1: equ $004D
PWPER2: equ $004E
PWCLK:  equ $0040       ; PWM clocks and concatenate (used to concat ch0 and ch1)
PWDTY0: equ $0050       ; PWM duty cycle for channel 1
PWDTY1: equ $0051
PWDTY2: equ $0052
PWPOL : equ $0041       ; PWM clock select and polarity
PWSCAL0:equ $0044       ; PWM Scale register 0
PWCTL : equ $0054       ; PWM control register


	; The next lines set up and start PWM 0
	movb #$00, PWCTL        ; Left aligned pulses
	movb #$00, PWCLK        ; Do not Concat channels 0 & 1, 2 & 3
	movb #$11, PWPOL        ; S0 is the source clock for ch1
	movb #$F0, PWSCAL0      ; Set up an initial frequency
	movb #$0F, PWEN         ; Enable PWM
	movb #$A5, PWPER0       ; Set channel 0 period
return: movb #$10, PWDTY0       ; Set channel 0 duty cycle -(right turn)-
	jsr delay               ; go to delay
middle: movb #$18, PWDTY0       ; Set channel 0 duty cycle -(straight ahead)-
	jsr delay               ; go to delay
left:   movb #$20, PWDTY0       ; Set channel 0 duty cycle -(left turn)-
	jsr delay               ; go to delay
mid:    movb #$18, PWDTY0       ; Set channel 0 duty cycle -(full steam 
				;       ahead)-
	jsr delay               ; go to delay
eof:    bra return              ; start again



;*******************************************************
;Delay routine to enable testing of full range of motion
;*******************************************************
delay:  ldx  #$07FF             ; x count
decrx:  dex                     ; decrement x
	ldy  #$0FFF             ; y count
decry:  dey                     ; decrement y
	cpy #$00                ; end of inner loop?
	bne decry               ; no, decrement y again
	cpx #$00                ; end of outer loop?
	bne decrx               ; no, decrement x again
	rts                     ; end delay