list p=16F84a
radix hex
title "SERVO"
#include <p16f84a.inc>
__config _CP_OFF & _PWRTE_OFF & _WDT_OFF & _HS_OSC
; servo control 1-2ms pulse every 20ms
cblock 0x0C
W_TEMP
STATUS_TEMP
servo0 ;pin rb0 position 0-255
servo1
servo2
servo3 ;pin rb3
intservogoto
endc
; org 0x2100 ;initial values for timer H and L bytes (10 seconds)
; de 0x00,0x0a ;first 2 bytes of eeprom
;----------------------------------------------------------------------
org 0x000
goto init
org 0x004 ;interrupt goes here
MOVWF W_TEMP ; Copy W to TEMP register,
SWAPF STATUS, W ; Swap status to be saved into W
MOVWF STATUS_TEMP ; Save status to STATUS_TEMP register
; Interrupt Service Routine
;writes intservergoto to program counter to send timing pulses to 4 servos in sequence
;without any messy logic. Initial value of intservergoto 09h. Timing is for 4MHz clock
;0 in servo variables gives about 2ms pulse, 254 gives about 1ms
;Don't use 255 for servo value
;Advantages: fast, not many files used
;Disadvantages: uses lots of program memory, hard to modify
movfw intservogoto
movwf PCL
call prescaler4 ;PCL 09h servo0
bsf PORTB,0 ;start pulse and next interrupt after 1ms
movlw 0E
movwf intservogoto
goto endint
movfw servo0 ;PCL 0E
movwf TMR0 ;variable part of pulse 0-1ms
movlw 13
movwf intservogoto
goto endint
call prescaler16 ;PCL 13
movlw 30 ;for 3.5ms pause before next servo
movwf TMR0
bcf PORTB,0 ;end pulse
movlw 1A
movwf intservogoto
goto endint
call prescaler4 ;PCL 1A servo1
bsf PORTB,1 ;start pulse and next interrupt after 1ms
movlw 1F
movwf intservogoto
goto endint
movfw servo1 ;PCL 1F
movwf TMR0 ;variable part of pulse 0-1ms
movlw 24
movwf intservogoto
goto endint
call prescaler16 ;PCL 24
movlw 30 ;for 3.5ms pause before next servo
movwf TMR0
bcf PORTB,1 ;end pulse
movlw 2B
movwf intservogoto
goto endint
call prescaler4 ;PCL 2B servo2
bsf PORTB,2 ;start pulse and next interrupt after 1ms
movlw 30
movwf intservogoto
goto endint
movfw servo2 ;PCL 30
movwf TMR0 ;variable part of pulse 0-1ms
movlw 35
movwf intservogoto
goto endint
call prescaler16 ;PCL 35
movlw 30 ;for 3.5ms pause before next servo
movwf TMR0
bcf PORTB,2 ;end pulse
movlw 3C
movwf intservogoto
goto endint
call prescaler4 ;PCL 3C servo3
bsf PORTB,3 ;start pulse and next interrupt after 1ms
movlw 41
movwf intservogoto
goto endint
movfw servo3 ;PCL 41
movwf TMR0 ;variable part of pulse 0-1ms
movlw 46
movwf intservogoto
goto endint
call prescaler16 ;PCL 46
movlw 30 ;for 3.5ms pause before next servo
movwf TMR0
bcf PORTB,3 ;end pulse
movlw 09
movwf intservogoto
goto endint
prescaler4
bsf STATUS, RP0 ; bank 1
movlw B'10000001' ; prescaler 4
movwf OPTION_REG ;
bcf STATUS, RP0 ; bank 0
return
prescaler16
bsf STATUS, RP0 ; bank 1
movlw B'10000011' ; prescaler 16
movwf OPTION_REG ;
bcf STATUS, RP0 ; bank 0
return
endint
; should configure Bank as required
;
SWAPF STATUS_TEMP,W ; Swap nibbles in STATUS_TEMP register
; and place result into W
MOVWF STATUS ; Move W into STATUS register
; (sets bank to original state)
SWAPF W_TEMP, F ; Swap nibbles in W_TEMP and place result in W_TEMP
SWAPF W_TEMP, W ; Swap nibbles in W_TEMP and place result into W
clrf INTCON ;clr interupts
bsf INTCON,T0IE ;enable timer interupt
retfie
init ;initialise stuff here
;sets interupts only on timer0 internal and prescaler 256
bsf STATUS, RP0 ; bank 1
movlw B'10000111' ; rtcc inc = tcylc/256 = tclk/(4*256)
movwf OPTION_REG ;
bcf STATUS, RP0 ; bank 0
movlw 09
movwf intservogoto
clrf TMR0 ; reset timer (and prescaler!)
movlw B'10100000' ; enable timer interrupt and GIE
movwf INTCON ;
BCF STATUS, RP0 ; bank 0
CLRF PORTB ; Initialize PORTB by
; clearing output
; data latches
BSF STATUS, RP0 ; Select Bank 1
MOVLW B'11110000' ; Value used to
; initialize data
; direction
MOVWF TRISB ; Set RB<0-3> as output, all others input
;porta should all be inputs anyway
BCF STATUS, RP0 ;bank 0
start
goto start
end
by Jeff Child
by Aaron Carman
by Jake Hertz
by Duane Benson