; SIMPLE SPEED CONTROL SOFTWARE TO CONTROL MOTOR & REVERSING RELAY
; 电机速度控制
;R/C 潜艇应用 4Mhz 16F84
;RA0 RA1速度控制. RA2伺服输入 .报警输入RA3. PortC is relay output.
;Alarm input for pressure switch etc
;(Slow forward when alarm input is 0v)

LIST P=16F84
#INCLUDE "p16F84.inc"

temp5 equ 0x20 ;File reg for temp storage
temp1 equ 0x21 ;File reg for temp storage
temp2 equ 0x22
temp3 equ 0x23
temp4 equ 0x24
OP_PULSE equ 0x25 ;File reg for out pulse value
ip_pulse equ 0x26 ;File reg for transmitter input pulse value
COUNTR equ 0x27
COUNTR1 equ 0x28

AVE equ 0x2A ;Used by FILTER
NEW equ 0x2B ;Used by FILTER
AVFRAC equ 0x2C ;Used by FILTER

#DEFINE servo_op H0 ;set bit 0 for servo o/p
#DEFINE servo_om H1 ;set bit 1 for servo o/p
#DEFINE pulse_ip H2 ;set bit 2 for pulse i/p
#DEFINE pressure H3 ;set bit 3 for pressure alarm i/p

;====================================================
ORG 0
start nop
clrf PORTA
clrf PORTB

bcf STATUS,RP0
movlw B00001100
tris PORTA
movlw B00000000
tris PORTB

start2 nop

main2 nop
btfsc PORTA,pulse_ip
goto main2

call meas_ip

movf ip_pulse,0
movwf NEW
call FILTER
movf AVE,0
movwf ip_pulse

call DATPROC

call LOOK ;look up motor speed
MOVWF OP_PULSE


call LOOK1 ;look up motor direction
MOVWF PORTB
;**********
;ALARM TEST
;**********
call ALARM

call output ;routine to output pulse

bcf PORTA,servo_op ;set motor output lo
bcf PORTA,servo_om ;set motor output lo

goto start2 ;go and do it all again

;**************************************************************************=
;* Measure transmitter input pulse, value in ip_pulse =
;* 0 = 864uS and 255 = 2139uS measured to the nearest 5uS =
;**************************************************************************

meas_ip nop
movlw 0xFE
movwf temp5
movwf temp1

meas_1 nop ;10 steps
btfss PORTA,pulse_ip ;is i/p lo
goto meas_1 ;yes wait for it to go hi

movlw .216 ; 730uS
movwf temp5
TimeA2 nop
decfsz temp5
goto TimeA2

btfss PORTA,pulse_ip
goto short_pulse

bsf PORTA,servo_op ;Put motor on
bsf PORTA,servo_om

clrf ip_pulse

Time_ip btfss PORTA,pulse_ip ;test the ip is still hi
goto end_ip ;go and test again
incfsz ip_pulse,1
goto Time_ip
goto max_pulse

end_ip
retlw 0
max_pulse
movlw HFB ;YES set reg to 250
movwf ip_pulse
retlw 0

short_pulse
movlw H1
movwf ip_pulse
retlw 0

;**************************************************************************=
;* Output pulse for motor = (Motor is already on.)
;**************************************************************************=
output nop
movf OP_PULSE,0 ;w from Hewitt replaced by 0
movwf temp3

opr_2 nop ;output loop OP_PULSE*5uS
nop
call DELAY
decfsz temp3,1
goto opr_2

retlw 0

;*******************************************
;Read most significant nibble of input value
;*******************************************
DATPROC MOVLW B11110000
BCF STATUS,0 ;Reset C flag to 0
ANDWF ip_pulse,1

BCF STATUS,0 ;Reset C flag to 0
rrf ip_pulse,1 ;divide by 2 and leave in register
BCF STATUS,0 ;Reset C flag to 0
rrf ip_pulse,1 ;divide by 2 and leave in register
BCF STATUS,0 ;Reset C flag to 0
rrf ip_pulse,1 ;divide by 2 and leave in register

movlw 0x01
addwf ip_pulse,1 ;Add 1 to give correct look up
MOVF ip_pulse,0
movwf ip_pulse
RETLW 0

;***********************************************
;Lookup table for motor speeds. (Use hex values)
;***********************************************
LOOK movf ip_pulse,0
ADDWF PCL,1
RETLW 33 ;1
RETLW 33 ;2
RETLW 32 ;3
RETLW 30 ;4
RETLW 2D ;5
RETLW 2A ;6
RETLW 27 ;7
RETLW 24 ;8
RETLW 21 ;9
RETLW 1E ;10
RETLW 1B ;11
RETLW 18 ;12
RETLW 15 ;13
RETLW 12 ;14
RETLW 0F ;15
RETLW 0C ;16
RETLW 0A ;17
RETLW 06 ;18
RETLW 01 ;19
RETLW 01 ;20
RETLW 01 ;21
RETLW 01 ;22
RETLW 01 ;23
RETLW 01 ;24
RETLW 01 ;25
RETLW 0A ;26
RETLW 13 ;27
RETLW 21 ;28
RETLW 24 ;29
RETLW 28 ;30
RETLW 28 ;31
RETLW 28 ;32

;*******************************************
;Lookup table for motor direction (Use hex)
;*******************************************
LOOK1 movf ip_pulse,0
ADDWF PCL,1
RETLW 0xFF ;1
RETLW 0xFF ;2
RETLW 0xFF ;3
RETLW 0xFF ;4
RETLW 0xFF ;5
RETLW 0xFF ;6
RETLW 0xFF ;7
RETLW 0xFF ;8
RETLW 0xFF ;9
RETLW 0xFF ;10
RETLW 0xFF ;11
RETLW 0xFF ;12
RETLW 0xFF ;13
RETLW 0xFF ;14
RETLW 0xFF ;15
RETLW 0xFF ;16
RETLW 0xFF ;17
RETLW 0xFF ;18
RETLW 0xFF ;19
RETLW 0xFF ;20
RETLW 0xFF ;21
RETLW 0xFF ;22
RETLW 0 ;23
RETLW 0 ;24
RETLW 0 ;25
RETLW 0 ;26
RETLW 0 ;27
RETLW 0 ;28
RETLW 0 ;29
RETLW 0 ;30
RETLW 0 ;31
RETLW 0 ;32

;**************************************************************************=
;LONG DELAY ROUTINE
;**************************************************************************=

;DELAY(uS) = COUNTR*((5*COUNTR1)+5)
;Adjust COUNTR1 to set up for particular R/C equpt.
DELAY
MOVLW H1 ;1 outer loops
MOVWF COUNTR
MOVLW H82 ;130 inner loops.
MOVWF COUNTR1
DELAY2 NOP
NOP ;3 cycle inner loop
DELAY3 DECFSZ COUNTR1,1 ;Counts down. Skip when 0
GOTO DELAY3 ;Loop
DECFSZ COUNTR,1 ;Counts down. Skip when 0
GOTO DELAY2 ;Loop
RETLW 0

;**********
;ALARMS
;**********
ALARM BTFSS PORTA,pressure ;Skip if 1
GOTO SAFE
retlw 0

SAFE MOVLW 0x06
MOVWF OP_PULSE
MOVLW 0x00
MOVWF PORTB
retlw 0

;------------------------------------
;FILTER (Average of last 16 numbers.)
;------------------------------------
FILTER
movf AVE,0 ;Moves AVE into W
subwf NEW,1 ;NEW - AVE (NEW - W)

swapf NEW,0 ;Swap upper and lower nibbles

andlw 0x0F ;get lower nibble(/16 int part)
skpc ;result is neg?
iorlw 0xF0 ;yes
addwf AVE,F

swapf NEW,W
andlw 0xF0 ;get /16 frac part
addwf AVFRAC,F
skpnc
incf AVE,F
RETLW 0

END

  

• 评论内容...
• 匿名发表
• [添加到收藏夹]
• 发表评论：(匿名发表无需登录，已登录用户可直接发表。) 登录状态：未登录