; 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