;****** INTERRUPT ROUTINE ******
;Interrupt Handler - come here every 18 msec to update servo positions
int movwf _w ;save the registers
movf status, w
movwf _status
movlw 0x020 ;disable further interrupts
movwf intcon
; movlw H'0F' ;turn on all the pulses
; iorwf portb
bsf porta,S0
bsf porta,S1
bsf porta,S2
bsf portb,S3
;Output a 1ms start pulse to each servo (4MHz crystal)
;Radio control servos require a 1mS hi pulse before the command pulse
movlw 143 ;wait 143x7us=1 msec
movwf count
goto $ + 1 ;seven cycle delay
goto $ + 1
decfsz count
goto $ - 3
;Output the command pulse to each servo (4MHz crystal)
;The value range of each servo is between D0 to D50. It takes 20uS to
;complete one pass of the loop, so the output to the servo is
;between 0x20us=0mS and 50x20us=1mS. The code can drive 4 servos.
movlw D'50' ;load master count
movwf count ;
intloop ;
movf count, w ;fetch master count
subwf servopc, w ;subtract from servo count
btfsc status, C ;done 50?
bcf porta, S0 ;yes, clear pitch command pulse
movf count, w ;no, keep pulse going and
subwf servor, w ;do next servo
btfsc status, C
bcf porta, S1 ;roll servo
movf count, w
subwf servoy, w
btfsc status, C
bcf porta, S2 ;yaw servo
movf count, w
subwf servox, w
btfsc status, C
bcf portb, S3
nop ;a 4th servo (bcf porta,SX)
goto $ + 1 ;a two cycle delay
decfsz count
goto intloop ;do the next count
movlw 0x000 ;turn off servo lines
andwf porta
;wait another 20ms
movlw 178 ;see above
movwf TMR0
movlw 0x0A0 ;re enable interrupt
movwf intcon
intend ;tidy and exit
movf _status, w
movwf status
swapf _w
swapf _w, w
retfie