Hi All,
I'm putting this up in assembler for any other amateur PIC enthusiasts who may be interested in something similar for PICs. It worked OK but I now use the Alf B and John R mod of the car level controller. Used last in 2010 and with a Freescale MMA7341L accelerometer. I'm sure that accelerometer technology has moved on since and the price will have dropped significantly!
David
;----------------------------------------------------------------------------------------
;Submarine level control - using ADC input from accelerometer and stick input on one chip
;----------------------------------------------------------------------------------------
;File:Accel 6.asm
;(c) David Forrest
;Date of this revision:24/3/2010
;Prescaler for TMR0 set to 128. 64 time too short.
;30/3/2010 ADC and eeprom now working
;30/3/2010 servo input now working. Good progress!
;30/3/2010 Reference voltage tried for ADC but GP1 needed for programming?
;31/3/2010 Switchover at stick midpoint now working
;1/4/2010 Freescale MMA7341L now being used. AVE8 used.
;2/4/2010 This version now tried on Resurgam with Futaba 9C transmitter. Works very well
;6/4/2010 Max and Min put into Accel input
;25/4/2010 Now tested in Charlie 2 at Barrow.Works very well even at high speed.
;**************************************************************************=
; Configure
;**************************************************************************=
list p=12f675 ; list directive to define processor
#include <p12f675.inc> ; processor specific variable definitions
__CONFIG _CP_OFF & _WDT_OFF & _BODEN_ON & _PWRTE_ON & _INTRC_OSC_NOCLKOUT & _MCLRE_OFF & _CPD_OFF
radix dec
errorlevel -302 ;Suppress banksel warnings
;**************************************************************************=
temp equ 0x20 ;File reg for temp storage
op_pulse equ 0x21 ;File reg for out pulse value
ip_pulse equ 0x22 ;File reg for transmitter input pulse value
COUNTR equ 0x23
COUNTR1 equ 0x24
a_pulse equ 0x25 ;
temp1 equ 0X26 ;servo movement increment
flag1 equ 0x27 ;flag register
temp2 equ 0x28
left equ 0x29
right equ 0x2A
ahead equ 0x2B
temp3 equ 0x2C
GPIOT equ 0x2D
medval equ 0x2E
NEW equ 0x2F
FLAGS equ 0x30
AVE equ 0x31
SPEED equ 0x32
;Storage for interrupt
TMR0_T equ 0x33
W_TEMP equ 0x34
STATUS_SAVE equ 0x35
GPIO_T equ 0x36
ADHI equ 0x37 ;
ADLO equ 0x38 ;
a_pulse_T equ 0x39 ;
op_pulse_T equ 0x3A ;
STATUS_T equ 0x4D
;GPIO_T equ 0x51
AVFRAC equ 0x3B ;For AVE16 subroutine
INPUT equ 0x3C ;For Top Tail
MINIMUM equ 0x3D ;For Top Tail
MAXIMUM equ 0x3E ;For Top Tail
;Lost signal registers
lostsig EQU 0x3F ;lost signal loop counter
lostsig1 EQU 0x40 ;lost signal loop counter
lostsig2 EQU 0x41
lostsig3 EQU 0x42 ;lost signal loop counter
setreg EQU 0x43 ;set bit counter for bit stream
clrreg EQU 0x44 ;clear bit counter for bit stream
TOCK EQU 0x45
TOCK1 EQU 0x46
TOCKCOP1 EQU 0x47
FLAG EQU 0x48
POSITION EQU 0x49
STORE1 EQU 0x4A ;EEprom storage
EMPTSTOP EQU 0x4B ;Tank empty position
FULLSTOP EQU 0x4C ;Tank full position
FLAG1 EQU 0x4E ;Flag for switching
EEINCR EQU 0x4F ;Increment the EEaddress
INPUT1 EQU 0x50 ;
;MAXIMUM EQU 0x52 ;
ERRSIG EQU 0x53 ;Difference between stick input and position
FLAG2 EQU 0x54 ;
DEADBAND EQU 0x55 ;
;FLAG2 EQU 0x56 ;
;FLAG2 EQU 0x57 ;
;FLAG2 EQU 0x58 ;
#DEFINE accel_ip H'0' ;GP0 as Accel input
#DEFINE servo_ip H'2' ;
#DEFINE servo_op H'4' ;
#DEFINE AVER H'1' ;average subroutine flags
#DEFINE STUP H'2' ;startup flags
#DEFINE BANK0 BCF STATUS,RP0
#DEFINE BANK1 BSF STATUS,RP0
;#DEFINE AVER H'1' ;average subroutine flags
;#DEFINE STUP H'2' ;startup flags
;#DEFINE ON H'0' ;switch on flag
;#DEFINE timer_on H'0'
;************************************************************
; Reset and Interrupt Vectors
;************************************************************
org 00h ; Reset Vector
goto Start
org 04h ; Interrupt vector
goto Intserv
;************************************************************
;**************************************************************************=
; Main program
;*************************************************************************=
Start
;Startup delay (1.25 mS)
call Delay
;Zero registers
CLRF temp1
CLRF temp2
CLRF left
CLRF right
CLRF ahead
CLRF temp3
CLRF medval
CLRF COUNTR
CLRF COUNTR1
CLRF a_pulse
CLRF NEW
CLRF AVE
CLRF SPEED
CLRF temp
CLRF op_pulse
CLRF ip_pulse
CLRF FLAGS
CLRF NEW
CLRF AVE
CLRF SPEED
CLRF TMR0_T
CLRF W_TEMP
CLRF GPIO_T
CLRF lostsig
CLRF lostsig1
CLRF lostsig2
CLRF lostsig3
CLRF setreg
CLRF clrreg
CLRF TOCK
CLRF TOCK1
CLRF TOCKCOP1
CLRF FLAG
CLRF POSITION
CLRF EMPTSTOP
CLRF FULLSTOP
CLRF INPUT
CLRF FLAG1
CLRF INPUT
CLRF MINIMUM
CLRF MAXIMUM
BSF STATUS,RP0 ;set bank 1
CLRF EEADR
BCF STATUS,RP0 ;set bank 0
CLRF FLAG2
CLRF ERRSIG
CLRF POSITION
movlw 01h
movwf EEINCR
;Activate next 4 lines if OSCCAL has been lost
; bsf STATUS, RP0 ;Bank 1
; call 3FFh ;Get the cal value
; movwf OSCCAL ;Calibrate
; bcf STATUS, RP0 ;Bank 0
movlw B'00000111' ;
movwf CMCON ;Disable comparators
movlw B'00000001' ;A/D on for channel 0.Left justfd
movwf ADCON0
BCF STATUS,RP0 ;set bank 0
CLRF GPIO ;clear register before setting ports
BANK1
movlw B'01010001' ;Fosc /16
movwf ANSEL ;Set AN0 as analogue input
movlw B'00000101'
movwf TRISIO ;write to GPIO
movlw B'0000101' ;Prescaler set to 1:64.WPU enabled
movwf OPTION_REG
MOVLW B'10010000' ;Global Intrpt and External Interrupt on
MOVWF INTCON
movlw B'00000000' ;No individual weak pull ups
; movlw B'00111110' ;Set individual weak pull ups
movwf WPU
BANK0
MOVLW B'10100000' ;Intpt on Timer overflow
MOVWF INTCON
CLRF temp2
CLRF flag1
movlw D'100'
movwf COUNTR ;Charge up startup delay counter
BSF FLAGS,STUP ;Startup flag
;**************************************************************************=
; MAIN PROGRAM LOOP
;**************************************************************************=
main NOP ;Main program is just a loop.
goto main
;**************************************************************************=
;Interrupt service routine
;**************************************************************************=
Intserv nop
;Output pulse for servo
;Save interrupt variables
movf W,W
movwf W_TEMP ;Store variables
movf STATUS,W
movwf STATUS_T
movf TMR0,W
movwf TMR0_T
movf GPIO,W
movwf GPIO_T
movf op_pulse,W
movwf op_pulse_T
;Measure input pulse
call meas_ip
;Measure accelerometer input pulse
bsf ADCON0,1 ;AD on
wait nop
btfsc ADCON0,1
goto wait
movf ADRESH,W
movwf ADHI
;Test - To reverse accelerometer direction
; comf ADHI,1 ;Take complement
;Test
; movlw D'200' ;was 170
; movwf ADHI
;Put minimum and maximum into ADHI
movf ADHI,0
movwf INPUT
call TOPTAIL
movf INPUT,0
movwf ADHI
;Averaging for accelerometer output
movf ADHI,0
movwf NEW
call AVE8 ;
movf AVE,0
movwf ADHI
;Put servo input into servo output
movf ip_pulse,0
movwf op_pulse
;If stick is in neutral use accelerometer input
BCF STATUS,0 ;Reset C flag to 0
;Lower switchpoint
MOVLW D'125' ;Load switchpoint 100
SUBWF ip_pulse,0 ;input-switchpoint.If +ve,C=1
BTFSS STATUS,0 ;Look at C bit
goto Leaper
GOTO Lower ;Do this if C=0
Lower nop
;Upper switchpoint
MOVLW D'185' ;Load switchpoint 140
SUBWF ip_pulse,0 ;input-switchpoint.If +ve,C=1
BTFSC STATUS,0 ;Look at C bit
goto Leaper
;Put accelerometer input in as output
movf ADHI,0
movwf op_pulse
Leaper nop
;Eeprom storage, timer skip routine reduces no of eeprom writes
decfsz temp2,1
goto skipof
call datae ;Store data in EEprom
; call EEWrite
movlw D'255' ;
movwf temp2
skipof nop
movlw D'160' ;213
movwf temp
bsf GPIO,servo_op
opr_1 nop
nop
decfsz temp,1 ;loop 213*5uS = 1065uS
goto opr_1
movf op_pulse,0 ;w from Hewitt replaced by 0
movwf temp ;
;get here after 864uS
opr_2 nop ;output loop op_pulse*5uS
nop
nop
decfsz temp,1
goto opr_2
bcf GPIO,servo_op ;set servo output lo
;**************************
;Restore variables
;**************************
movf STATUS_T,W
movwf STATUS
movf TMR0_T,W
movwf TMR0
movf GPIO_T,W
movwf GPIO
movf W_TEMP,W
movwf W
movf op_pulse_T,W
movwf op_pulse
;Return from Interrupt
BCF INTCON,2
retfie
;**************************************************************************=
;* Measure transmitter input pulse, value in ip_pulse =
;* 0 = 864uS and 255 = 2139uS measured to the nearest 5uS =
; (Original routine by Ken Hewitt. With lost signal routine, taken from switcher program)
;**************************************************************************
meas_ip nop
;INPUTS PORTA,servo_ip
;OUTPUTS ip_pulse
clrf ip_pulse ;clear the file reg
;Load up counters(temp and temp1)
movlw D'216' ;was 216
movwf temp ;Load up temp counters
clrw
meas_1 nop ;Lost signal routine. If no input, increment lost signal counters
; INCF lostsig,1 ;Add 1 to lostsig - this section commented out to reduce loop time
; BCF STATUS,2
; movf lostsig,0
; sublw D'255' ;Timer minus 255
; btfsc STATUS,2 ;skip if clear
INCF lostsig1,1 ;clock countr div by 255
BCF STATUS,2
MOVF lostsig1,0 ;Copies lostsig1
sublw D'255' ;Timer minus 255
btfsc STATUS,2
INCF lostsig2,1 ;clock countr div by 255
BCF STATUS,2
MOVF lostsig2,0 ;Copies lostsig1
sublw D'255' ;Timer minus 255
btfsc STATUS,2
goto fail_safe
btfss GPIO,servo_ip ;wait for ip line to be low before doing anything, then continue
goto meas_1 ;
clrf lostsig ;If we've got to here, we have an input ,so clear lost sig counters
clrf lostsig1
clrf lostsig2
clrf lostsig3
clrf FLAGS
;Time incoming pulse. Wait 864 uS if pulse is <864 uS then error
TimeA2 nop
decfsz temp,1 ;loop 216*4uS = 864 uS
goto TimeA2
;Next test to ensure ip is still hi
btfss GPIO,servo_ip
goto short_pulse
;Loop to meas input (first test at 1000 uS) this loop is 5uS long
;250*5 us = 1250us
Time_ip btfss GPIO,servo_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 of ip pulse
end_ip nop
;NO return with measured value in reg
goto out
max_pulse
movlw D'252' ;YES set reg to 252
movwf ip_pulse
goto out
short_pulse
movlw D'5' ;
movwf ip_pulse ;
goto out
fail_safe NOP ;Once all the lostsigy cntrs full, come here
MOVLW D'250' ;servo failsafe position, was 250
MOVWF ip_pulse
out nop
return
;----------------------------
; Delay routine (1.25mS)
;----------------------------
;Startup delay (1.25 mS)
Delay movlw D'250' ;
movwf temp
opr_3 nop
nop
decfsz temp,1 ;loop 250*5uS
goto opr_3
return
;**************************************************************************=
;Create a 15 mS delay
Delay1 MOVLW D'15'
MOVWF temp
Delay1a NOP
DECFSZ temp1,1 ;4uS x 256 loops=approx 1mS
GOTO Delay1a
DECFSZ temp,1
GOTO Delay1a
return
;**************************************************************************=
; Delay for slow speed
;**************************************************************************
Delayslo nop
MOVLW B'00000000' ;Turn LEDs off
MOVWF GPIO
call Delay
call Delay
call Delay
; call Delay
; call Delay
; call Delay
return
;**************************************************************************=
; Data for EEWrite routine
;**************************************************************************
datae nop
movlw D'0' ;
movwf EEINCR
movf ip_pulse,0
call EEWrite
movlw D'1' ;
movwf EEINCR
movf ADHI,0
call EEWrite
leapy10
return
;-------------------------------------------------
;The sub-routine to write to EEPROM is shown below.
EEWrite nop
bsf STATUS,RP0 ; Bank 1
bsf EECON1,WREN ; Enable write
bcf INTCON,GIE ; Disable INTs
movwf EEDATA ; set EEPROM data
movf EEINCR,0 ; ee address in
movwf EEADR ; set EEPROM address
movlw 0x55
movwf EECON2 ; Write 55h
movlw 0xAA
movwf EECON2 ; Write AAh
bsf EECON1,WR ; Set WR bit
bsf INTCON,GIE ; Enable INTS
; begin write
bcf STATUS,RP0 ; Bank 0
ret12 btfss PIR1,EEIF ; wait for write to complete.
goto ret12
bcf PIR1,EEIF ; and clear the 'write complete' flag
bsf INTCON,GIE ; Enable Global Interrupt
bsf STATUS,RP0 ; Bank 1
bcf EECON1,WREN ; Disable write
bcf STATUS,RP0 ; Bank 0
retlw 0x00
EE_Read bsf STATUS,RP0 ; Bank 1
movlw D'0'
movwf EEADR ; Address to read
bsf EECON1,RD ; EE Read
movf EEDATA,0 ; W = EEDATA
bcf STATUS,RP0 ; Bank 0
movwf EEDATA
retlw 0x00
;**************************************************************************=
;Program clock pulse routine
;**************************************************************************=
TICKER nop
call Delay ;Padding
call Delay ;Padding
INCF TOCK,1 ;Add 1 to TOCK
BCF STATUS,2
movf TOCK,0
sublw D'255' ;Timer minus 255
btfsc STATUS,2
INCF TOCK1,1 ;clock countr div by 255
MOVF TOCK1,0 ;Copies Tock1 (About 6 secs)
MOVWF TOCKCOP1
RETURN
;**************************************************************************=
;Divide or multiply routine
;**************************************************************************=
divide nop
rrf NEW,0 ;divide by 2 and leave in W
andlw B'01111111' ;mask off one upper bit
; rlf NEW,0 ;multiply by 2 and leave in W
; andlw B'11111110' ;mask off one lower bit
MOVWF NEW
return
;------------------------------------
;AVERAGE (Average of last 8 numbers.)
;------------------------------------
; Needs NEW as input. Gives AVE as output
; The formula can be rewritten as: Ave' = Ave + ( New - Ave )/8 ;
AVE8 MOVF AVE,0
SUBWF NEW,0 ;NEW - AVE, saved in W
BTFSC STATUS,C ;Check the carry bit
GOTO PLUS2 ;Its positive
GOTO MINUS2 ;Its negative
PLUS2
MOVWF NEW ;Save NEW-AVE
BCF FLAGS,AVER
GOTO EXIT1
MINUS2 MOVF NEW,0 ;
SUBWF AVE,0 ;AVE-NEW
MOVWF NEW
BSF FLAGS,AVER
EXIT1 NOP ;Continue
rrf NEW,0 ;divide by 2 and leave in W
rrf NEW,0 ;divide by 2 and leave in W
rrf NEW,0 ;divide by 2 and leave in W
andlw B'00011111' ;mask off three upper bits
MOVWF NEW
BTFSC FLAGS,AVER
GOTO NEG3
GOTO POS3
NEG3 MOVF NEW,0
SUBWF AVE,1 ;AVE-NEW
GOTO LOOPY3
POS3 MOVF NEW,0
ADDWF AVE,1 ;AVE+NEW
LOOPY3 nop
RETURN
; ------------------------------------------------------------------------
; TOP & TAIL SUBROUTINE. Needs INPUT, MINIMUM & MAXIMUM
;--------------------------------------------------------------------------
;TOP & TAIL the INPUT
TOPTAIL MOVLW D'100' ; was 110 Minimum
MOVWF MINIMUM ;Puts value into MINIMUM
MOVLW D'180' ;was 170 Maximum
MOVWF MAXIMUM
;If CARRY BIT (C) is set (1), Make it maximum
BCF STATUS,0 ;Reset C flag to 0
BTFSC STATUS,0
CALL MAXSUB
;If INPUT LT MINIMUM, MAKE IT MINIMUM.
MOVF MINIMUM,0 ;Get MINIMUM and put in W
BCF STATUS,0 ;Reset C flag to 0
SUBWF INPUT,0 ;Calc INPUT-MINIMUM (W).If +ve,C=1
BTFSS STATUS,0 ;Look at C bit
CALL MINSUB ;Do this if C=0
;If INPUT GT MAXIMUM, MAKE IT MAXIMUM
MOVF INPUT,0 ;Get INPUT and put in W
BSF STATUS,0 ;Reset C flag to 1
SUBWF MAXIMUM,0 ;Calc MAXIMUM-INPUT(W).If +ve,C=1
BTFSS STATUS,0 ;Look at C bit
CALL MAXSUB ;Do this if C=0
BCF STATUS,0 ;Reset C flag to 0.Imptnt for RRF!
RETURN ;End of TOPTAIL
;Subroutines for TOPTAIL
;-----------------------
;If INPUT is too low. Put a minimum into INPUT
MINSUB MOVF MINIMUM,0
MOVWF INPUT
BCF STATUS,0 ;Reset C flag to 0.Imptnt for RRF!
RETURN
;If INPUT too high. Put a maximum into INPUT
MAXSUB MOVF MAXIMUM,0
MOVWF INPUT
BCF STATUS,0 ;Reset C flag to 0.Imptnt for RRF!
RETURN
END
;**************************************************************************=
;Bit value mode finder
;**************************************************************************=
;Finds mode value for a stream of bits
;Needs medval, bit 1 and edit COUNTR1 (254 most sensitive)
median btfss medval,1 ;if bit is set. accumulate clear counter
call clearacc
btfsc medval,1 ;if bit is set. accumulate set counter
call setacc
decfsz COUNTR1,1 ;reset counters etc if zero
goto skipper
call TESTER
clrf clrreg
clrf setreg
movlw D'254'
movwf COUNTR1
skipper return
clearacc incf clrreg,1
return
setacc incf setreg,1
return
TESTER BCF STATUS,C
MOVF clrreg,0 ;Copies clrreg
subwf setreg,0 ;subtract one from another and leave result in w
btfsc STATUS,C
GOTO PLUS_ER ;Its positive
GOTO MINUS_ER ;Its negative
PLUS_ER BSF medval,1
GOTO CE
MINUS_ER BCF medval,1
CE RETURN
; ------------------------------------------------------------------------
; TOP & TAIL SUBROUTINE. Needs INPUT, MINIMUM & MAXIMUM
;--------------------------------------------------------------------------
;TOP & TAIL the INPUT
TOPTAIL MOVLW D'5' ;102 Minimum
MOVWF MINIMUM ;Puts value into MINIMUM
MOVLW D'250' ;153 Maximum
MOVWF MAXIMUM
;If CARRY BIT (C) is set (1), Make it maximum
BTFSC STATUS,0
CALL MAXSUB
;If INPUT LT MINIMUM, MAKE IT MINIMUM.
MOVF MINIMUM,0 ;Get MINIMUM and put in W
BCF STATUS,0 ;Reset C flag to 0
SUBWF INPUT,0 ;Calc INPUT-MINIMUM (W).If +ve,C=1
BTFSS STATUS,0 ;Look at C bit
CALL MINSUB ;Do this if C=0
;If INPUT GT MAXIMUM, MAKE IT MAXIMUM
MOVF INPUT,0 ;Get INPUT and put in W
BSF STATUS,0 ;Reset C flag to 1
SUBWF MAXIMUM,0 ;Calc MAXIMUM-INPUT(W).If +ve,C=1
BTFSS STATUS,0 ;Look at C bit
CALL MAXSUB ;Do this if C=0
BCF STATUS,0 ;Reset C flag to 0.Imptnt for RRF!
RETURN ;End of TOPTAIL
;Subroutines for TOPTAIL
;-----------------------
;If INPUT is too low. Put a minimum into INPUT
MINSUB MOVF MINIMUM,0
MOVWF INPUT
BCF STATUS,0 ;Reset C flag to 0.Imptnt for RRF!
RETURN
;If INPUT too high. Put a maximum into INPUT
MAXSUB MOVF MAXIMUM,0
MOVWF INPUT
BCF STATUS,0 ;Reset C flag to 0.Imptnt for RRF!
RETURN
;**************************************************************************
;Latch pump on routine
;**************************************************************************
; Latch keeps flag off for 2 seconds and then flag on for a maximum
; of 90 seconds. Flag is than off for 4 mins and then everything starts again
latch nop
;TEST
; movlw D'255'
; movwf TICKER
; Short starting delay - flag off
movf TOCKCOP1,0
sublw D'1' ;Timer minus 1.(This keeps pump off for 4 secs)
btfsc STATUS,0 ;Check the carry bit
goto leapy ;not timed out so keep motor on
;Flag on
movf TOCKCOP1,0
sublw D'20' ;Timer minus 20.(This puts pump on for 1mins)
btfsc STATUS,0 ;Check the carry bit
goto leapy1 ;not timed out so keep motor on
;Flag off for 4 minutes
movf TOCKCOP1,0
sublw D'70' ;Timer minus 70. Tis gives 5 min pause
btfsc STATUS,0 ;Check the carry bit
goto leapy2 ;not timed out so keep motor on
;Reset timers and start again
CLRF TOCK1
CLRF TOCKCOP1
bsf FLAG,timer_on
goto leapy3
leapy bcf FLAG,timer_on ;initial delay not over so keep flag off
goto leapy3
leapy1 nop
bsf FLAG,timer_on ;not timed out so keep flag on
goto leapy3
leapy2 bcf FLAG,timer_on ;timed out so keep flag off
goto leapy3
leapy3 RETURN
;------------------------------------
;SUBROUTINE
;AVERAGE (Average of last 4 numbers.)
;------------------------------------
; Needs NEW as input. Gives AVE as output. Also FLAGS.
; The formula can be rewritten as: Ave' = Ave + ( New - Ave )/4 ;
AVE4 MOVF AVE,0
SUBWF NEW,0 ;NEW - AVE, saved in W
BTFSC STATUS,C ;Check the carry bit
GOTO PLUS ;Its positive
GOTO MINUS ;Its negative
PLUS
MOVWF NEW ;Save NEW-AVE
BCF FLAGS,AVER
GOTO EXIT
MINUS MOVF NEW,0
SUBWF AVE,0 ;AVE-NEW
MOVWF NEW
BSF FLAGS,AVER
EXIT NOP ;Continue
rrf NEW,0 ;divide by 2 and leave in W
rrf NEW,0 ;divide by 2 and leave in W
andlw B'00111111' ;mask off two upper bits
MOVWF NEW
BTFSC FLAGS,AVER
GOTO NEG1
GOTO POS1
NEG1 MOVF NEW,0
SUBWF AVE,1 ;AVE-NEW
GOTO LOOPY1
POS1 MOVF NEW,0
ADDWF AVE,1 ;AVE+NEW
LOOPY1 nop
RETURN
;------------------------------------
;AVERAGE (Average of last 8 numbers.)
;------------------------------------
; Needs NEW as input. Gives AVE as output
; The formula can be rewritten as: Ave' = Ave + ( New - Ave )/8 ;
AVE8 MOVF AVE,0
SUBWF NEW,0 ;NEW - AVE, saved in W
BTFSC STATUS,C ;Check the carry bit
GOTO PLUS2 ;Its positive
GOTO MINUS2 ;Its negative
PLUS2
MOVWF NEW ;Save NEW-AVE
BCF FLAGS,AVER
GOTO EXIT1
MINUS2 MOVF NEW,0 ;
SUBWF AVE,0 ;AVE-NEW
MOVWF NEW
BSF FLAGS,AVER
EXIT1 NOP ;Continue
rrf NEW,0 ;divide by 2 and leave in W
rrf NEW,0 ;divide by 2 and leave in W
rrf NEW,0 ;divide by 2 and leave in W
andlw B'00011111' ;mask off three upper bits
MOVWF NEW
BTFSC FLAGS,AVER
GOTO NEG3
GOTO POS3
NEG3 MOVF NEW,0
SUBWF AVE,1 ;AVE-NEW
GOTO LOOPY3
POS3 MOVF NEW,0
ADDWF AVE,1 ;AVE+NEW
LOOPY3 nop
RETURN
;------------------------------------
;AVERAGE (Average of last 16 numbers.)
;------------------------------------
; Initially written by Andrew Warren (fastfwd at ix.netcom.com).
; Optimized by Dmitry A. Kiryashov (zews at aha.ru) 06/18/2000
; The formula can be rewritten as: Ave' = Ave + ( New - Ave )/16 ;
; 12 clocks/words.
; Needs NEW as input and AVFRACGives AVE as output
AVE16
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
RETURN
Yesterday at 9:21 pm by geofrancis
» 868/915 Mhz as a viable frequency for submarines.
Sun Sep 08, 2024 10:55 pm by geofrancis
» WW2 mini sub build
Thu Sep 05, 2024 8:15 am by david f
» Trumpeter 1/144 PLAN Type 092 Xia Class SSBN
Tue Aug 06, 2024 5:42 am by redboat219
» UHF radio control for submarines (openLRS, LoRa, FSK etc. on 458Mhz and 433Mhz )
Sat Jul 27, 2024 9:05 am by david f
» David Forrest's R class
Fri Jul 19, 2024 1:00 pm by david f
» Futaba -868/915mhz equipment
Sun Jun 09, 2024 10:47 pm by tsenecal
» Darnell type 21 submarine, need some help
Sun Jun 09, 2024 9:35 am by Deep Diver (Fred)
» bladder bags
Tue May 28, 2024 8:30 am by Deep Diver (Fred)