; 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 H'0' ;set bit 0 for servo o/p
#DEFINE servo_om H'1' ;set bit 1 for servo o/p
#DEFINE pulse_ip H'2' ;set bit 2 for pulse i/p
#DEFINE pressure H'3' ;set bit 3 for pressure alarm i/p

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

bcf STATUS,RP0
MOVlw B'00001100'
tris PORTA
MOVlw B'00000000'
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 H'FB' ;YES set reg to 250
MOVwf ip_pulse
retlw 0

short_pulse
MOVlw H'1'
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 B'11110000'
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 H'1' ;1 outer loops
MOVWF COUNTR
MOVLW H'82' ;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