;*******************************************************************
; IR_FILTER.ASM
; sample AD channel x and 篩選出 頻率of puck and goals
; 16f73用SPI 方式通過 SSP 組件與 PIC16C74A 聯系
;*******************************************************************
;歌林電子制作實驗室www.nbglin.com 
;TEL：0574-88464538 email:nbpic@126.com
;
; portc<1> determines if the puck value值在 LEDs顯示
; portc<2> determines which (if the puck is not shown) of the two goal
; values is shown on the LEDs, and switches the order they are sent to
; the 74A host.
; 第一最后發送 (the defensive goal) is shown on the LEDs (if the
; the puck is not)
; portc<0> determines if the low or high byte is shown on the LEDs
;*******************************************************************
;

LIST w=1, n=0 
LIST p=16C73A 
__CONFIG b'11111110110010'
#INCLUDE "P16C73A.INC" 

w_temp equ 0x20 
goal2_b equ 0x30 ; beginning of goal2 bins
goal2_e equ 0x5F ; end of 24 two bytegoal2 bins
goal1_b equ 0x60 ; beginning of goal1 bins
goal1_e equ 0x7F ; end of 16 two byte goal1 bins
goal1_out_l equ goal1_b ; output from algorithm
goal1_out_h equ goal1_b+1; output from algorithm
goal2_out_l equ goal2_b ; output from algorithm
goal2_out_h equ goal2_b+1; output from algorithm

; Bank 1

CBLOCK 0xA1
puck_acc_l, puck_acc_h ; puck accumulator for past ad values
goal1_acc_l, goal1_acc_h; goal1 accumulator for past ad values
goal2_acc_l, goal2_acc_h; goal2 accumulator for past ad values
dc_acc_l, dc_acc_h ; accumulator for all past ad values
puck_ptr ; pointer to puck bins
goal1_ptr ; pointer to goal1 bins
goal2_ptr ; pointer to goal2 bins
past_ptr ; poiter to last of the saved AD values
fpuck_past_ptr ; pointer to first saved AD val used for puck
fgoal1_past_ptr ; pointer to first saved AD val used for goal1
fgoal2_past_ptr ; pointer to first saved AD val used for goal2
ENDC

pastval_b equ 0xB0 ; beginning of past AD values
pastval_e equ 0xBF ; end of 16 saved past AD values
puck_b equ 0xC0 ; beginning of puck bins
puck_e equ 0xFF ; end of the 32 two byte puck bins 
puck_out_l equ puck_b ; output from algorithm
puck_out_h equ puck_b+1; output from algorithm

; misc constants
ad_cycle equ d'30' ; 30 us sampling period
num_ad equ d'96' ; number of samples in one run
spi_delay equ 30 ; extra delay between sent bytes in us
;
;
; ***************************** Reset vector

ORG 0x0000
reset clrf INTCON ; disable interrupts
goto start

; ***************************** Interrupt vector

ORG 0x0004
interrupt
goto reset ; interrupts should be disabled

;****************************** Initialization code

ORG 0x0020
start
BANKSEL PORTB
clrf PORTB
clrf PORTC
BANKSEL TRISB
movlw 0x00
movwf TRISB
movlw b'00010111'
movwf TRISC
init_ad ; set up AD module on all channels
; channel 0 active
init_timer1_ad ad_cycle ; set up AD sampling every ad_cycle us 
init_spi ; set up SPI communication
init_timer0 ; set up timer0 as timer w/o prescaler

;****************************** Main routine 

main 
BANKSEL ad_cnt
clrf ad_cnt
clear_regs goal1_b, goal1_e
clear_regs goal2_b, goal2_e
BANKSEL puck_acc_l
clrf puck_acc_l
clrf puck_acc_h
clrf goal1_acc_l
clrf goal1_acc_h
clrf goal2_acc_l
clrf goal2_acc_h
clrf dc_acc_l
clrf dc_acc_h
clear_regs pastval_b, pastval_e
clear_regs puck_b, puck_e
movlw puck_b - 2
movwf puck_ptr
movlw goal1_b - 2
movwf goal1_ptr
movlw goal2_b - 2
movwf goal2_ptr
movlw pastval_b - 1
movwf past_ptr
movlw pastval_b - d'16'
movwf fpuck_past_ptr
movlw pastval_b - d'8'
movwf fgoal1_past_ptr
movlw pastval_b - d'12'
movwf fgoal2_past_ptr
BANKSEL ADCON0 
wait_on btfss ADCON0, GO ; skip if a conversion is in progress
goto wait_on ; wait until conversion is started
goto wait_ad

check_ad
btfss ADCON0, GO ; skip if a conversion is in progress
call panic ; conversion already finished, we're
; too slow, give up!!!
wait_ad btfsc ADCON0, GO ; skip if the conversion is finished
goto wait_ad ; wait until conversion is finished

; update pointer to last AD value
BANKSEL past_ptr
incf past_ptr, f
movlw (pastval_e + 1) & 0xFF
subwf past_ptr, w
btfss STATUS, Z
goto cont1 ; ptr is not yet pastval_e+1 
movlw pastval_b ; wrap!
movwf past_ptr
cont1 movf past_ptr, w
movwf FSR
BANKSEL ADRES
movf ADRES, w
movwf INDF ; puts most recent AD value in list of past values 
; add most recent AD value to accumulators
BANKSEL puck_acc_l ; We're now in bank1 and will remain so until after handle_target
add_w_to_16 puck_acc_l ; add value pointed at by past_ptr (and FSR) to puck_acc
add_w_to_16 goal1_acc_l ; add value pointed at by past_ptr to goal1_acc
add_w_to_16 goal2_acc_l
add_w_to_16 dc_acc_l

; macro invoked once for each target (goal1, goal2, and puck)
handle_target MACRO f_past_pt, targ_ptr, targ_b, targ_e, acc_l
LOCAL cont1, cont2, cont3
; update pointer to first AD value in target accumulator
incf f_past_ptr, f
movlw (pastval_e + 1) & 0xFF
subwf f_past_ptr, w
btfss STATUS, Z
goto cont1 ; ptr is not yet pastval_e+1 
movlw pastval_b ; wrap!
movwf f_past_ptr
cont1 ; ...and to next bin
incf targ_ptr, f 
incf targ_ptr, f 
movlw (targ_e + 1) & 0xFF 
subwf targ_ptr, w
btfss STATUS, Z 
goto cont2 
movlw targ_b ; targ_ptr=targ_e+1 ==> wrap!
movwf targ_ptr
cont2 ; add target accumulator value to next bin
movf targ_ptr, w
movwf FSR
add_16_to_ind16 acc_l ; add contents of acc to bin pointed at by targ_ptr
; remove the oldest AD value in the target accumulator if it is old enough
movlw pastval_b
subwf f_past_ptr, w
btfss STATUS, C
goto cont3 ; C is clear ==> pastval_b is greater than ptr 
movf f_past_ptr, w ; ptr is legal ==> it points at an old enough value
movwf FSR
sub_ind_fr_16 acc_l 
cont3
ENDM

handle_goal1
handle_target fgoal1_past_ptr, goal1_ptr, goal1_b, goal1_e, goal1_acc_l
handle_goal2
handle_target fgoal2_past_ptr, goal2_ptr, goal2_b, goal2_e, goal2_acc_l
handle_puck
handle_target fpuck_past_ptr, puck_ptr, puck_b, puck_e, puck_acc_l

; loop back
BANKSEL ad_cnt
incf ad_cnt, f
movlw num_ad
subwf ad_cnt, w
btfss STATUS, C
goto check_ad ; C is clear ==> ad_cnt has not reached num_ad

; ca 141 instr ==> ca 28,2 us

BANKSEL fgoal1_past_ptr ; bank 1 as required by the handle_target macro 
sublw 6
btfsc STATUS, C
goto handle_goal1 ; C is set ==> 6 >= ad_cnt-num_ad 
; ad_cnt-num_ad >= 7 ==> goal1 accumulator is zero
addlw 4 ; 4 + 6-(ad_cnt-num_ad) 
btfsc STATUS, C
goto handle_goal2 ; C is set ==> 10-(ad_cnt-num_ad) >= 0 
; ad_cnt-num_ad >= 11 ==> goal1 accumulator is zero
addlw 4 ; 4 + 10-(ad_cnt-num_ad) 
btfsc STATUS, C
goto handle_puck ; C is set ==> 14-(ad_cnt-num_ad) >= 0 
; ad_cnt-num_ad >= 15 ==> all target accumulators are zero; we're finished

;now find the bins with the greatest accumulated values
find_greatest16 goal1_b, goal1_e
find_greatest16 goal2_b, goal2_e
find_greatest16 puck_b, puck_e

BANKSEL dc_acc_l
bcf STATUS, C
rrf dc_acc_h, f
rrf dc_acc_l, f ; dc_acc = dc_acc/2

movlw dc_acc_l
movwf FSR
BANKSEL goal1_out_l
sub_ind16_fr_16 goal1_out_l
btfsc STATUS, C
goto after_sub_goal1
clrf goal1_out_l ; result negative (it should not) ==> set to zero
clrf goal1_out_h
after_sub_goal1
sub_ind16_fr_16 goal2_out_l
btfsc STATUS, C
goto after_sub_goal2
clrf goal2_out_l ; result negative (it should not) ==> set to zero
clrf goal2_out_h
after_sub_goal2
BANKSEL puck_out_l
sub_ind16_fr_16 puck_out_l
btfsc STATUS, C
goto after_sub_puck
clrf puck_out_l ; result negative (it should not) ==> set to zero
clrf puck_out_h
after_sub_puck

; put one of the results (high or low byte) on portb
; portc <1><2> equal to...
; 00:puck, 01:puck, 10:goal2, 11:goal2
BANKSEL PORTC
movlw goal2_out_l
movwf FSR
movlw goal1_out_l
btfsc PORTC, 2
movwf FSR
movlw puck_out_l
btfss PORTC, 1 
movwf FSR 
btfss PORTC, 0
incf FSR, f ; access high byte of result if portc <0> is clear
movf INDF, w
movwf PORTB

; send data to host PIC

VARIABLE i=1
WHILE i <=2 ; send 0xFF twice
BANKSEL SSPBUF
movf SSPBUF, w ; fetch received value
movlw 0xFF ; start by sending two 0xFF's
movwf SSPBUF ; transmit
wait_bf ; wait for transmission to complete
wait_us spi_delay ; wait spi_delay us
i++ 
ENDW
i=1
WHILE i <= 4 ; send 4 data words
; load address
IF i==1
movlw puck_out_l
ENDIF
IF i==2
movlw goal2_out_l
BANKSEL PORTC
btfsc PORTC, 2 ; portc<2> switrchrd the order... 
movlw goal1_out_l
ENDIF
IF i==3
movlw goal1_out_l
BANKSEL PORTC
btfsc PORTC, 2 ; ...of goal1 and goal2 
movlw goal2_out_l
ENDIF
IF i==4
movlw dc_acc_l
ENDIF
movwf FSR ; point at low byte
BANKSEL SSPBUF
movf SSPBUF, w ; fetch received value
movf INDF, w ; fetch low byte
movwf SSPBUF ; transmit 
wait_bf ; wait for transmission to complete
wait_us spi_delay ; wait spi_delay us
movf SSPBUF, w ; fetch received value
; also bank 0
movwf ad_source ; store it this time (should be received every time)
incf FSR, f ; point at high byte
movf INDF, w ; fetch high byte
movwf SSPBUF ; transmit 
wait_bf ; wait for transmission to complete
wait_us spi_delay ; wait spi_delay us
i++ 
ENDW

; ensure enough time to set AD input channel
BANKSEL T1CON
bcf T1CON, TMR1ON ; stop timer1
BANKSEL TMR1H 
clrf TMR1H 
clrf TMR1L ; clear it
BANKSEL ADCON0 
bcf ADCON0, GO ; stop any AD conversion
BANKSEL ad_source
movf ad_source, w ; load received source pin number
sublw 4 
btfss STATUS, C 
goto after_adsel ; C is clear ==> 4 < ad_source ; not valid
; copy 3 lsb's to CHS2:CHS0
BANKSEL ADCON0
bcf ADCON0, CHS0 
bcf ADCON0, CHS1
bcf ADCON0, CHS2
btfsc ad_source, 0 ; same bank (0)
bsf ADCON0, CHS0 
btfsc ad_source, 1 
bsf ADCON0, CHS1
btfsc ad_source, 2 
bsf ADCON0, CHS2
after_adsel

; start timer1 again 
BANKSEL T1CON
bsf T1CON, TMR1ON 

; begin again
goto main


panic movlw 0xFF ; hang with PORTB LEDs half lit
BANKSEL PORTB
movwf PORTB
nop
clrf PORTB
goto panic


END