; ; wand.asm - main wand code ; ; Copyright (C) 2006 Nathan (Acorn) Pooley ; ; This program is free software; you can redistribute it and/or ; modify it under the terms of the GNU General Public License ; version 2 as published by the Free Software Foundation. ; ; This program is distributed in the hope that it will be useful, ; but WITHOUT ANY WARRANTY; without even the implied warranty of ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ; GNU General Public License (gpl.txt) for more details. ; ; You should have received a copy of the GNU General Public License ; along with this program; if not, write to the Free Software ; Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. ; ; You can contact the author, Nathan (Acorn) Pooley, by writing ; to Nathan (Acorn) Pooley, 949 Buckeye Drive, Sunnyvale, CA 94086, USA ; or through the form at http://www.rawbw.com/~acorn/wand/feedback.html ; ;#@DOC@ This is the main wand program ;########################################################################### ;################################ KERNEL STUFF ############################# ;########################################################################### #define HACK1 0 #define DBPRINT 1 #define SHOW_LED_DURING_MOTION 0 #define USE_CONST_MAGSQ_TH 1 #define QUIT_WHEN_SAMPLE_FULL 0 #define ENAB_BLINKS 0 #define ENAB_ISR_TAP 0 #define ENAB_OLD_TAP_HANDLE 0 #define ENAB_W_PARSE 0 #define ENAB_OLD_MOTION_SPELLS 0 #define ENAB_FMTB_MOTION_SPELLS 1 #define ENAB_TESTS 0 #define DBG_SPELL_MSG_IS_SPELL_NAME 0 ; ; this enables extra org directives to keep code from shifting around ; #define DBORG 0 #include "chip.inc" #include "kernel.inc" radix dec expand ;########################################################################### ;################################ ACCELEROMETER VALUES ##################### ;########################################################################### ; ; Still values ; ; x min = 0x4e ; x mid = 0x55 or 0x56 ; x max = 0x5c ; ; y min = 0x4e ; x mid = 0x55 or 0x56 ; y max = 0x5c ; ; small movements ; ; x = 51 - 5a ; y = 4e - 51 ; ; ; tap ; see TAPS under TIMING CHART ; fast rise time ; mag = 60 = 0x3c ; mag*mag = 0xe10 ; ; ; ; Accelerometer threshhold defaults ; ACC_THESH_X_AVG equ 0x55 ACC_THESH_X_MIN equ 0x4e ACC_THESH_X_MAX equ 0x5c ACC_THESH_Y_AVG equ 0x4f ACC_THESH_Y_MIN equ 0x4e ACC_THESH_Y_MAX equ 0x5c ; ; initial threshhold values ; ACC_INIT_AVGX equ 0x55 ; average x value (initial) ACC_INIT_AVGY equ 0x4f ; average y value (initial) ; ; Time to ignore false-taps after a tap (in sample times - 640usec) ; ACC_TAP_MASK_TIME equ 312 ; counts to mask sampler after a tap #if 1 ACC_INIT_TH_MAGSQ_TAP equ (0x20*0x20) ; min mag squared for tap ACC_INIT_TH_MAGSQ_Z equ (7*7)-1 ; min mag squared to start sampling ;ACC_INIT_TH_MAGSQ_S equ (5*6)-1 ; max mag squared to stop sampling ACC_INIT_TH_MAGSQ_S equ (2*2)-1 ; max mag squared to stop sampling ACC_TH_MAGSQ_WAKE equ (19*19) ; min magsq to wake up ; ; threshholds for saving an angle state or a pause state ; TH_IMP is impulse threshhold ; TH_TIME is time threshhold in sample-times (640us per sample) ; TH_MAG is magnitude threshhold ; ACC_TH_TIME_END equ 0x0927 ; end when still for this long (1.5sec) ACC_TH_TIME_RETIRE equ 0x009c ; max time to keep cur state w/out seeing it ACC_TH_TIME_PAUSE equ 0x009c ; min time to register a pause state ACC_TH_TIME_RETIRE_TAP equ 0x0005 ; max time to keep tap state w/out seeing it ;ACC_TH_TIME_RETIRE_TAP equ 0x009c ; max time to keep tap state w/out seeing it ; ; to save a sample one of these conditions must be met ; - magnitude > ACC_TH_MAG_TAP ; - time and impulse exceed threshholds ; - time and magnitude exceed threshholds ; ;ACC_TH_TIME_SAVE equ 0x0004 ; min time to register a new state ACC_TH_TIME_SAVE equ 0x0004 ; min time to register a new state ;ACC_TH_IMP_SAVE equ 0x0020 ; min impulse to register a new state ACC_TH_IMP_SAVE equ 0x0080 ; min impulse to register a new state ACC_TH_MAG_SAVE equ 0x0015 ; min impulse to register a new state ACC_TH_MAG_TAP equ 0x0020 ; mag to consider as a tap (for rythm) ACC_TH_MAG_DRAW equ 0x0006 ; min delta mag for draw enable ; ACC_TH_MAGSQ_STILL - max magsq to consider still (for ending spell cast) ACC_TH_MAGSQ_STILL equ (18*18) #else ; ; special values for debug taps only ; ACC_INIT_TH_MAGSQ_TAP equ (0x20*0x20) ; min mag squared for tap ACC_INIT_TH_MAGSQ_Z equ (0x20*0x20)-1 ; min mag squared to start sampling ACC_INIT_TH_MAGSQ_S equ (0x20*0x20)-1 ; max mag squared to stop sampling ACC_TH_TIME_END equ 0x0927 ; end when still for this long (1.5sec) ACC_TH_TIME_RETIRE equ 0x0001 ; max time to keep cur state w/out seeing it ACC_TH_TIME_PAUSE equ 0x0001 ; min time to register a pause state ACC_TH_TIME_RETIRE_TAP equ 0x0001 ; max time to keep tap state w/out seeing it ACC_TH_TIME_SAVE equ 0x0001 ; min time to register a new state ACC_TH_IMP_SAVE equ 0x0080 ; min impulse to register a new state ACC_TH_MAG_SAVE equ 0x0015 ; min impulse to register a new state ACC_TH_MAG_TAP equ 0x0020 ; mag to consider as a tap (for rythm) #endif ;########################################################################### ;################################ REGISTER SETTINGS ######################## ;########################################################################### ; ; ADCON0_OFF = 0000 0000 ; 7-6 reserved 00 ; 5-2 CHS3-0 0000 select channel 0 ; 1 GO/DONE 0 do not start conversion ; 0 ADON 0 A/D unit off ; ; ; Acc-X: (A) AN0 0000 0001 ; Acc-Y: (D) AN1 0000 0101 ; Acc-Xmin: (C) AN3 0000 1101 ; Acc-Xmax: (B) AN2 0000 1001 ; Acc-Ymin: (F) AN4 0001 0001 ; Acc-Ymax: (E) AN12 0011 0001 ; ADCON0_OFF equ 0x00 ADCON0_ACCX equ 0x01 ADCON0_ACCY equ 0x05 ADCON0_ACCXMIN equ 0x0d ADCON0_ACCXMAX equ 0x09 ADCON0_ACCYMIN equ 0x11 ADCON0_ACCYMAX equ 0x31 #define b_adgo ADCON0,1 ; ; ADCON1 = 0000 0000 ; 7-6 reserved 00 ; 5 VCFG1 0 Vref- = Vss ; 4 VCFG0 0 Vref+ = Vdd ; 3-0 PCFG3-0 0000 all available pins set as analog inputs ; REGVAL_ADCON1 equ 0x00 ; ; ADCON2 = 0001 0001 ; 7 ADFM = 0 left justified formt ; 6 reserved = 0 ; 5-3 ACQT2-0 = 010 4 Tad cycles for acquisition ; 2-0 ADCS2-0 = 001 A/D conversion clock = Fosc/8 (Tad = 2usec) ; ; Tad = 2usec ; 4 Tad for acquire ; 11 Tad for convert ; 15 Tad total = 30usec per convert ; REGVAL_ADCON2 equ 0x11 ; ; T3CON = 1011 1001 ; 7 RD16 = 1 16 bit read/write mode ; 6 T3CCP2 = 0 Timer1 is the clock source for CCP1 ; 5-4 RD16 = 11 1:8 prescaler ; 3 T3CCP1 = 1 Timer3 is the clock source for CCP2 ; 2 _T3SYNC = 0 unused in Fosc/4 mode ; 1 TMR3CS = 0 use Fosc/4 oscillator ; 0 TMR3ON = 1 Tim ; ; Used to count up to the a/d conversion. CCP2 clears the counter. ; REGVAL_T3CON_OFF equ 0x00 REGVAL_T3CON_AD equ 0xb9 ; ; CCP resets Timer3 & starts A/D conversion evey 640usec ; Fosc/4 = 1usec period ; 1:8 prescale = 8usec period ; 640usec/8usec = 80 clocks ; REGVAL_CCPRL2H equ 0 REGVAL_CCPRL2L equ 80 ; ; CCP2CON = 0000 1011 ; 7-6 reserved = 00 ; 5-4 DC2B1-0 = 00 unused in compare mode ; 3-0 CCP2M3-0 = 1011 mode (compare mode + special event trigger) ; ; Used to start a/d conersion of accx when timer3 reaches the trigger val ; REGVAL_CCP2CON equ 0x0b ;########################################################################### ;########################################################################### ;################################ MACROS ################################### ;########################################################################### ;########################################################################### ; ; Macros ; ; SOUT1 - print string in w_str_base1 block of strings ; SOUT1CR - print string followed by CR ; ; MACROS FOR ADDING TEXT TO WAND OUTPUT ; ------------------------------------- ; ; WCLR - clear wand ; WCOUT - wand print character in w ; WSOUT0 - wand print string in kki_str_base0 block of strings ; WSOUT1 - wand print string in w_str_base1 block of strings ; WSOUT - wand print string ; ; ; SOUT1 - print string from w_str_base1 block of strings ; SOUT1 macro str_addr movlw str_addr-w_str_base1 rcall w_sout_base1 endm SOUT1L macro str_addr movlw str_addr-w_str_base1 call w_sout_base1 endm SOUT1CR macro str_addr movlw str_addr-w_str_base1 rcall w_sout_base1_cr endm SOUT1CRL macro str_addr movlw str_addr-w_str_base1 call w_sout_base1_cr endm ; ; WCLR - clear wand ; WCLR macro rcall w_wclear endm WCLRL macro call w_wclear endm ; ; WCOUT - wand print character in w ; WCOUT macro char movlw char rcall w_wcout endm WCOUTL macro char movlw char call w_wcout endm ; ; WSOUT - print string ; WSOUT macro str_addr movlw LOW str_addr movwf v_strl movlw HIGH str_addr rcall w_wsout_wh endm WSOUTL macro str_addr movlw LOW str_addr movwf v_strl movlw HIGH str_addr call w_wsout_wh endm ; ; WSOUT0 - print string from str_base0 string table ; WSOUT0 macro str_addr movlw str_addr-kki_str_base0 rcall w_wsout_base0 endm WSOUT0L macro str_addr movlw str_addr-kki_str_base0 call w_wsout_base0 endm ; ; WSOUT1 - print string from w_str_base1 block of strings ; WSOUT1 macro str_addr movlw str_addr-w_str_base1 rcall w_wsout_base1 endm WSOUT1L macro str_addr movlw str_addr-w_str_base1 call w_wsout_base1 endm ;########################################################################### ;################################ ERRORS & BREAKPOINTS ##################### ;########################################################################### ERROR_B1_ISR_OVERRUN equ 0xb1 ; ISR took too long ERROR_B2_DRAW2_AD_OVERRUN equ 0xb2 ; A/D not done when expected ;########################################################################### ;########################################################################### ;################################ VARIABLES ################################ ;########################################################################### ;########################################################################### ; ; v_* - wand specific ; ; kk_* - kernel public label ; vk_* - kernel public byte variables ; bk_* - kernel public bit variables ; ; kki_* - kernel private label ; vki_* - kernel private byte variables ; bki_* - kernel private bit variables ; ; vaddr - next free RAM byte ; ;########################################################################### ;################################ EEPROM LAYOUT ############################ ;########################################################################### ; EEPROM_END - first free eeprom address ;########################################################################### ;################################ PERSISTANT GLOBAL VARIABLES ############## ;########################################################################### ; ; These variables are preserved across warm boot. ; ;########################################################################### ;################################ VARIABLES ################################ ;########################################################################### v_bits1 equ vaddr+0 v_bits2 equ vaddr+1 v_bits3 equ vaddr+2 v_tmp equ vaddr+3 vaddr+=4 #define b_do_accx v_bits1,0 ; next A/D conversion is x #define b_accbuf_sample v_bits1,1 ; enable sampling into FSR2 buffer #define b_parse_done v_bits1,2 ; set when no more accbuf entries for parse #define b_parse_wag v_bits1,3 ; set when wagging back & forth #if ENAB_BLINKS #define b_blink v_bits1,4 ; set when v_blink_cnt reaches 0 #endif #if ENAB_ISR_TAP #define b_tap_mask v_bits1,5 ; ignore tap while set (after a tap) #define b_tap v_bits1,6 ; set when a tap occurs #define b_tap_enable v_bits1,7 ; taps are enabled when set #endif #define b_draw_ok v_bits2,0 ; draw enabled (w_draw2) #define b_draw_go v_bits2,1 ; draw going (w_draw2) #define b_draw_scroll v_bits2,2 ; scroll the text #define b_d2_maybe_tap v_bits2,3 ; tap might have occurred (w_draw2) #define b_d2_tap v_bits2,4 ; tap occurred (w_draw2) #define b_d2_sleep v_bits2,5 ; time to sleep (w_draw2) #define b_got_spell v_bits2,6 ; set if spell was recognized #define b_spell_yes_no v_bits3,0 ; current string is a yes/no question #define b_spell_abort v_bits3,1 ; abort spell cast #define b_qsectmr0_done v_bits3,2 ; quarter-second-timer 0 expired if set #define b_qsectmr1_done v_bits3,3 ; quarter-second-timer 1 expired if set #define b_acc_motion v_bits3,4 ; set in acc isr when motion detected while ; sampling #define b_acc_finish v_bits3,5 ; if set isr will stop sampling #define b_show_time v_bits3,6 ; set to show time #if ENAB_ISR_TAP v_tap_bits equ vaddr+0 vaddr+=1 ; ; These bits are set for 1 loop (from w_main_run until next w_main_run) ; #define b_tap_up v_tap_bits,0 ; set when a up tap occurs #define b_tap_right v_tap_bits,1 ; set when a right tap occurs #define b_tap_down v_tap_bits,2 ; set when a down tap occurs #define b_tap_left v_tap_bits,3 ; set when a left tap occurs #endif v_accx equ vaddr+0 ; recent x value v_accy equ vaddr+1 ; recent x value v_accx_min equ vaddr+2 ; min x value v_accx_max equ vaddr+3 ; min x value v_accy_min equ vaddr+4 ; min x value v_accy_max equ vaddr+5 ; min x value vaddr+=6 ;=========================================================================== ; acc sampling ;=========================================================================== v_absx equ vaddr+0 ; abs value of v_accx + v_avgx_neg v_absy equ vaddr+1 ; abs value of v_accy + v_avgy_neg vaddr+=2 v_angle equ vaddr+0 ; angle measured v_mag equ vaddr+1 ; magnitude (sqrt of magsq) v_magsql equ vaddr+2 ; magnitude squared (low byte) v_magsqh equ vaddr+3 ; magnitude squared (high byte) vaddr+=4 #if ENAB_ISR_TAP v_tap_angle equ vaddr+0 ; angle of most recent tap v_tap_mask_acntl equ vaddr+1 ; time left to mask acc (low) v_tap_mask_acnth equ vaddr+2 ; time left to mask acc (high) v_th_magsqh_tap equ vaddr+3 ; threshhold for v_magsqh to be a tap vaddr+=4 #endif v_avgx_neg equ vaddr+0 ; negative of average x value (-0x4f = 0xb1) v_avgy_neg equ vaddr+1 ; negative of average y value (-0x55 = 0xab) v_th_magsqh equ vaddr+2 ; threshhold for v_magsql to sample v_th_magsql equ vaddr+3 ; threshhold for v_magsql to sample ; (v_th_magsql_z or s) v_th_retimeh equ vaddr+4 ; max time to hold state without seeing it v_th_retimel equ vaddr+5 vaddr+=6 #if !USE_CONST_MAGSQ_TH v_th_magsql_z equ vaddr+0 ; v_th_magsql when prev == zero (-8*8 = -64 = 0xc0) v_th_magsql_s equ vaddr+1 ; v_th_magsql when prev != zero (-7*7 = -49 = 0xcf) vaddr+=2 #endif #if HACK1 ; debug v_hack1_magsql equ vaddr+0 v_hack1_magsqh equ vaddr+1 v_hack1_mag equ vaddr+2 v_hack1_angle equ vaddr+3 v_hack1_a equ vaddr+4 v_hack1_b equ vaddr+5 v_hack1_c equ vaddr+6 v_hack1_d equ vaddr+7 vaddr+=8 #endif ;=========================================================================== ; function parameters ;=========================================================================== v_strh equ vaddr+0 ; string high pointer v_strl equ vaddr+1 ; string low pointer vaddr+=2 ;=========================================================================== ; drawing ;=========================================================================== v_sleep_wait equ vaddr+0 ; how long until we sleep v_draw_cnt equ vaddr+1 ; time before next column of pixels v_draw_mask equ vaddr+2 ; which ixel to draw v_tap_wait equ vaddr+3 ; wait between draw and tap vaddr+=4 v_drawbuf_startl equ vaddr+0 v_drawbuf_starth equ vaddr+1 v_drawbuf_endl equ vaddr+2 v_drawbuf_endh equ vaddr+3 vaddr+=4 #define SCROLL_SPEED 1 #if SCROLL_SPEED v_scrollspd equ vaddr+0 vaddr+=1 #endif #if ENAB_BLINKS ;=========================================================================== ; blink counter ;=========================================================================== v_blink_cnt equ vaddr+0 ; countdown to next blink (640 usec counter) v_blink_ptr equ vaddr+1 ; where we are in blink sequence (0=off) vaddr+=2 #endif ;=========================================================================== ; time temporaries - see w_pause_w and w_timer_set_fsr0 ;=========================================================================== v_th equ vaddr+0 ; time v_tl equ vaddr+1 ; v_t0l equ vaddr+2 ; time of s<mins crossing v_t0h equ vaddr+3 ; v_dtl equ vaddr+4 ; deltaT = (t0-t1)/4 v_dth equ vaddr+5 ; v_qsectmr0 equ vaddr+6 ; quarter second timer (see w_qsectmr0_set) v_qsectmr1 equ vaddr+7 ; quarter second timer (see w_qsectmr1_set) vaddr+=8 ;=========================================================================== ; spell casting ;=========================================================================== v_spell_ptrh equ vaddr+0 ; spellinfo for cast spell v_spell_ptrl equ vaddr+1 ; v_spell_yesh equ vaddr+2 ; spellinfo for yes v_spell_yesl equ vaddr+3 ; v_spell_noh equ vaddr+4 ; spellinfo for no v_spell_nol equ vaddr+5 ; vaddr+=6 #if ENAB_W_PARSE v_spell_len equ vaddr+0 ; # of bytes in cast spell vaddr+=1 #endif ;=========================================================================== ; Buffers ;=========================================================================== RAMBUF_DRAW equ 0x300 ; pixels to draw (LEN=0x400) RAMBUF_DRAW_END equ 0xf00 ; end of pixel draw buffer RAMBUF_RYTHM equ 0x300 ; rythm buffer (LEN=0x100) RAMBUF_MOTION equ 0x300 ; motion buffer (LEN=0x100) RAMBUF_ACC equ 0x500 ; acceleration sample buffer (LEN=0x700) RAMBUF_ACC_END equ 0xf00 ; end of acceleration sample buffer ;=========================================================================== ; MEMORY MAP ;=========================================================================== ; ; 0x0000 - 0x007f - access bank RAM ; 0x0080 - 0x00ff - bank 0 RAM (currently accessed through BSR register) ; ; 0x0100 - 0x0dff - buffer mem (see below) ; ; 0x0e00 - 0x0eff - ACC buf (coulod be used for BSR space if needed) ; 0x0f00 - 0x0f7f - persistant wand RAM (if needed - unused for now) ; 0x0f80 - 0x0fff - Special Function Registers (access bank addressable) ; ; BUFFERS ; 0x0100 - 0x01ff - TX ; 0x0200 - 0x02ff - RX ; 0x0300 - 0x03ff - DRAW M2LIST RYTHM ; 0x0400 - 0x04ff - DRAW M2LIST ; 0x0500 - 0x05ff - DRAW ACC M2LIST ; 0x0600 - 0x06ff - DRAW ACC M2LIST ; 0x0700 - 0x07ff - DRAW ACC M2LIST STBL2 ; 0x0800 - 0x08ff - DRAW ACC M3LIST STBL1 ; 0x0900 - 0x09ff - DRAW ACC M3LIST SPELLDATA ; 0x0a00 - 0x0aff - DRAW ACC M3LIST MMATRIX ; 0x0b00 - 0x0bff - DRAW ACC M3LIST STOKENS ; 0x0c00 - 0x0cff - DRAW ACC M3LIST MTOKENS ; 0x0d00 - 0x0dff - DRAW ACC ; 0x0e00 - 0x0eff - DRAW ACC ; 0x0f00 - 0x0f7f - persistant ram (unused) ; ; ; ;=========================================================================== ; TIMING CHART ;=========================================================================== ; ; CLOCKS ; 1 us/cycle - program clock in fast mode ; 122 us/cycle - program clock in slow mode ; 250 ms - bk_qsec set every 1/4 sec (from main_run to main_run) ; 1 min - bk_minute set evry minute (from main_run to main_run) ; 1 hour - bk_hour set evry hour (from main_run to main_run) ; 244 usec - TMR1L increments with this period ; 62 msec - TMR1H increments with this period (TMR1L overflow) ; 16 sec - TMR1H overflows every 16 seconds ; ; A/D ; 30 usec - time to do one A/D conversion (fast mode) (CALCULATED) ; (NOTE: measured time indicated definitely 48usec or less) ; 640 usec - time between kicking off X A/D conversion (for ISR mode) ; 5 msec - time to settle after turning Accelerometer on ; 700 usec - time to settle after turning A/D converter on ; 3.5 msec - time to settle after turning LEDs on/off ; (NOTE: LEDs can change value by 3 units (steady state)) ; (NOTE: LEDs can change value by 15 units (dynamic)) ; 2 usec - Tad (a/d time) 4 + 11 = 15 Tad per conversion (4 for acquire) ; ; TAPS ; Good taps jump 70+ units - peak reached in under 1 ms. ; Cusion taps jump 70+ units - peak reached in under 10 ms. ; Non-taps jump up to 50 units peak reached in over 30 ms. ; ; DRAW ; 9 usec - time each LED is on (1 at a time) ; 52 usec - time to turn on & off all 5 LEDs once ; 1456 usec - column time ; 8736 usec - character time (incl 1-col gap) ; ; ;########################################################################### ;########################################################################### ;################################ CODE ##################################### ;########################################################################### ;########################################################################### ;########################################################################### ;################################ MAIN PROGRAM VECTORS ##################### ;########################################################################### org kk_app_main goto w_main org kk_app_isr_low return org kk_app_isr_high rcall isr_ad retfie 1 ;########################################################################### ;################################ INTERRUPTS ############################### ;########################################################################### ; ; ISR timing: ; ; conversion takes 30usec ; ; time in usec==cycles ; ; isr time section ; -------- ------------ ; 17 top of x ; 3 bottom of x ; 19 top of y (all y if no sampling) ; <=47 top of astate ; ? top of sample ; <=31 calc angle ; ? calc mag (sqrt) ; ? sample ; ; ; <start x convert> ; 30 x convert time ; 45 time until y convert starts ; 30 y convert time ; 169 time to handle x ; --- ; 274 total time ; ; ; <start x> occurs every 640 usec ; ;=========================================================================== ; A/D interrupt ;=========================================================================== isr_ad: bcf b_adif #if ENAB_BLINKS ; ; blink timer - set b_blink when v_blink_cnt reaches 0 ; btfss b_blink dcfsnz v_blink_cnt,f bsf b_blink #endif btg b_do_accx btfsc b_do_accx bra isr_ad_y isr_ad_x: movlw ADCON0_ACCY movwf ADCON0 movf ADRESH,w movwf v_accx movwf v_absx cpfsgt v_accx_max ; remember x, maxx, minx movwf v_accx_max cpfslt v_accx_min movwf v_accx_min isr_ad_x_starty: bsf b_adgo ; start y conversion return ; ; here we are ready to read y result ; isr_ad_y: movlw ADCON0_ACCX movwf ADCON0 movf ADRESH,w cpfsgt v_accy_max movwf v_accy_max cpfslt v_accy_min movwf v_accy_min movwf v_accy movwf v_absy #if ENAB_ISR_TAP ; ; decrement tap mask counter ; if a tap occurred this will clr tapmask when zero ; if a tap did not occur this does nothing ; decfsz v_tap_mask_acntl,f bra isr_ad_astate dcfsnz v_tap_mask_acnth,f bcf b_tap_mask ; timer ran out - start looking for taps again #endif ;=========================================================================== ; ; calculate astate (magnitude and angle) ; ; v_angle - angle measured ; v_mag - magnitude (sqrt of v_magsq) ; v_magsql - magnitude squared (low byte) ; v_magsqh - magnitude squared (high byte) ; v_absx - abs value of v_accx + v_avgx_neg ; v_absy - abs value of v_accy + v_avgy_neg ; ; astate values ; 0x00 - 0x1f angle 0-31 ; 0x82 zero magnitude ; 0x84 scrambled ; ; v_avgx_neg - negative of average x value (-0x4f = 0xb1) ; v_avgy_neg - negative of average y value (-0x55 = 0xab) ; v_th_magsql - threshhold for v_magsql to sample (v_th_magsql_z or s) ; v_th_magsql_z - v_th_magsql when prev == zero ; v_th_magsql_s - v_th_magsql when prev != zero #if ENAB_ISR_TAP ; v_th_magsqh_tap - threshhold for v_magsqh to be a tap (-0xe = 0xf2) #endif ; isr_ad_astate: clrf v_angle ; ; calc abs x ; isr_ad_absx: movf v_avgx_neg,w addwf v_absx,f bnn isr_ad_absy negf v_absx movlw -31 movwf v_angle ; ; calc abs y ; isr_ad_absy: movf v_avgy_neg,w addwf v_absy,f bnn isr_ad_mag negf v_absy movlw 15 addwf v_angle,f negf v_angle ; ; calculate magnitude ; isr_ad_mag: setf v_mag ; assume max magnitude movf v_absx,w mulwf v_absx movff PRODL,v_magsql movff PRODH,v_magsqh movf v_absy,w mulwf v_absy movf PRODL,w addwf v_magsql,f movf PRODH,w addwfc v_magsqh,f bnc isr_ad_mag2 setf v_magsql ; clamp to max magnitude setf v_magsqh isr_ad_mag2: #if HACK1 movff v_magsql,v_hack1_magsql movff v_magsqh,v_hack1_magsqh clrf v_hack1_mag clrf v_hack1_angle #endif ; ; request to stop sampling? ; btfsc b_acc_finish bra isr_ad_sample_zero ; ; is magnitude big enough to register? ; movf v_magsql,w addwf v_th_magsql,w movf v_magsqh,w addwfc v_th_magsqh,w bnc isr_ad_sample_zero ; small magnitude - call it 0 ; ; if mag < ACC_TH_MAGSQ_STILL then skip shrink ; if mag >= ACC_TH_MAGSQ_STILL then set b_accbuf_motion ; movlw LOW (-ACC_TH_MAGSQ_STILL) addwf v_magsql,w movlw HIGH (-ACC_TH_MAGSQ_STILL) addwfc v_magsqh,w bnc isr_ad_shrink_done ; small mag - no shrink needed bsf b_acc_motion ; big mag - set motion bit ; ; ensure absx and absy are less than 0x40 ; (divide both by 2 or 4 if necessary) ; isr_ad_shrink: movf v_absx,w iorwf v_absy,w andlw 0xc0 bz isr_ad_shrink_done rrncf v_absx,f rrncf v_absy,f bcf v_absx,7 bcf v_absy,7 bra isr_ad_shrink isr_ad_shrink_done: #if ENAB_ISR_TAP ; ; big enough to be a tap? ; movf v_magsqh,w addwf v_th_magsqh_tap,w bnc isr_ad_sample ; not big enough to be a tap? ; ; mag is big enough to count as a tap ; isr_ad_tap: btfsc b_tap_enable ; skip tap if not enabled btfsc b_tap_mask ; skip tap if still masked from previous tap bra isr_ad_sample ; ; New tap!! ; movlw (HIGH ACC_TAP_MASK_TIME)+1 movwf v_tap_mask_acnth movlw (LOW ACC_TAP_MASK_TIME)+1 movwf v_tap_mask_acntl bsf b_tap_mask bsf b_tap rcall isr_calc_angle ; calc angle movf v_angle,w movwf v_tap_angle btfss b_accbuf_sample bra isr_ad_done bra isr_ad_sample2 #else bra isr_ad_sample #endif ;=========================================================================== ; ; SAMPLING VARIABLES ; ; ; input variables ; ; v_mag - current magnitude ; v_angle - current angle (garbage if v_mag is 0) ; ; ; prev zero sample (before current sample) ; v_smplpz_cnth equ vaddr+0 ; # of zero samples in a row v_smplpz_cntl equ vaddr+1 ; vaddr+=2 ; ; current sample ; v_smplc_imph equ vaddr+0 ; total impulse v_smplc_impl equ vaddr+1 v_smplc_cnth equ vaddr+2 ; total time v_smplc_cntl equ vaddr+3 v_smplc_angle equ vaddr+4 ; original angle v_smplc_min_mag equ vaddr+5 ; min magnitude v_smplc_max_mag equ vaddr+6 ; max magnitude v_smplc_best_angle equ vaddr+7 ; angle of max magnitude v_smplc_flags equ vaddr+8 ; flags vaddr+=9 #define SMPLC_FLAGS_INIT 0x80 #define b_smplc_zero v_smplc_flags,0 ; includes zero samples ; ; next zero sample (since current sample) ; v_smplnz_cnth equ vaddr+0 ; # of zero samples in a row v_smplnz_cntl equ vaddr+1 ; vaddr+=2 ; ; new sample (since current sample) ; v_smpln_imph equ vaddr+0 ; total impulse v_smpln_impl equ vaddr+1 v_smpln_cnth equ vaddr+2 ; total time v_smpln_cntl equ vaddr+3 v_smpln_angle equ vaddr+4 ; original angle v_smpln_min_mag equ vaddr+5 ; min magnitude v_smpln_max_mag equ vaddr+6 ; max magnitude v_smpln_best_angle equ vaddr+7 ; angle of max magnitude vaddr+=8 ;=========================================================================== ; ; mag is too small to register - use mag==0 ; isr_ad_sample_zero: clrf v_mag ; ; use v_th_magsql_z (larger threshhold) for v_th_magsql ; #if USE_CONST_MAGSQ_TH movlw HIGH (-ACC_INIT_TH_MAGSQ_Z) movwf v_th_magsqh movlw LOW (-ACC_INIT_TH_MAGSQ_Z) movwf v_th_magsql #else movff v_th_magsql_z,v_th_magsql #endif btfss b_accbuf_sample bra isr_ad_done ; ; increment v_smplnz_cntl,h ; incf v_smplnz_cntl,f bnc isr_ad_sample_zero2 incf v_smplnz_cnth,f bnc isr_ad_sample_zero2 setf v_smplnz_cntl ; clamp to 21 sec max setf v_smplnz_cnth isr_ad_sample_zero2: #if SHOW_LED_DURING_MOTION bsf bk_led2 #endif ; ; request to finish up? ; btfsc b_acc_finish bra isr_ad_sample_finish movlw LOW (-ACC_TH_TIME_END) addwf v_smplnz_cntl,w movlw HIGH (-ACC_TH_TIME_END) addwfc v_smplnz_cnth,w bnc isr_ad_sample_zero_done ; ; still for >ACC_TH_TIME_END ; ensure we have at least 4 samples ; movlw LOW (-(RAMBUF_ACC+12)) addwf FSR2L,w movlw HIGH (-(RAMBUF_ACC+12)) addwfc FSR2H,w bnc isr_ad_sample_zero_done isr_ad_sample_finish: ; ; we have some samples and have been still ; - quit sampling ; - send any pending sample ; bcf b_accbuf_sample bra isr_ad_sample_sendresult isr_ad_sample_zero_done: bra isr_ad_done ;=========================================================================== ; ; calc & record angle & magnitude ; isr_ad_sample: btfss b_accbuf_sample bra isr_ad_done rcall isr_calc_angle ; calc angle isr_ad_sample2: rcall isr_calc_mag ; calc magnitude #if HACK1 movff v_mag,v_hack1_mag movff v_angle,v_hack1_angle #endif #if SHOW_LED_DURING_MOTION bcf bk_led2 #endif ; ; use v_th_magsql_s (smaller threshhold) for v_th_magsql ; #if USE_CONST_MAGSQ_TH movlw HIGH (-ACC_INIT_TH_MAGSQ_S) movwf v_th_magsqh movlw LOW (-ACC_INIT_TH_MAGSQ_S) movwf v_th_magsql #else movff v_th_magsql_s,v_th_magsql #endif ; ; do we have a current angle? (v_smplc_angle >= 0) ; btfsc v_smplc_angle,7 bra isr_ad_sample_check_new ; no - check for new state ; ; has it been a while since we saw a current angle? ; (compare smpln_cnt+smplnz_cnt to v_th_retime) ; movf v_th_retimel,w addwf v_smplnz_cntl,w btfss STATUS,C addwf v_smpln_cntl,w movf v_th_retimeh,w addwfc v_smplnz_cnth,w bc isr_ad_sample_check_new addwf v_smpln_cnth,w bc isr_ad_sample_check_new isr_ad_sample_check_current: ; ; are we within +/-1 of current angle? ; movf v_angle,w subwf v_smplc_angle,w bz isr_ad_sample_current btfsc WREG,7 negf WREG addlw -1 bz isr_ad_sample_current addlw -0x1f+1 bz isr_ad_sample_current isr_ad_sample_check_new: ; ; do we even have a new angle? ; btfsc v_smpln_angle,7 bra isr_ad_sample_newnew ; no - check for new state ; ; are we within +/-1 of new angle? ; movf v_angle,w subwf v_smpln_angle,w bz isr_ad_sample_samenew btfsc WREG,7 negf WREG addlw -1 bz isr_ad_sample_samenew addlw -0x1f+1 bz isr_ad_sample_samenew ; ; we are seeing a brand new new angle ; isr_ad_sample_newnew: ; ; count any new time as pause time ; movf v_smpln_cntl,w addwf v_smplnz_cntl,f movf v_smpln_cnth,w addwfc v_smplnz_cnth,f bnc isr_ad_sample_newnew2 setf v_smplnz_cntl ; clamp to max cnt setf v_smplnz_cnth isr_ad_sample_newnew2: movf v_angle,w movwf v_smpln_angle movwf v_smpln_best_angle movf v_mag,w movwf v_smpln_max_mag movwf v_smpln_min_mag movwf v_smpln_impl clrf v_smpln_imph movlw 1 movwf v_smpln_cntl clrf v_smpln_cnth bra isr_ad_done ;=========================================================================== ; we are still seeing the same angle ; isr_ad_sample_current: ; ; integrate impulse ; movf v_mag,w addwf v_smplc_impl,f clrf WREG addwfc v_smplc_imph,f bnc isr_ad_sample_current2 setf v_smplc_imph ; clamp to max impulse setf v_smplc_impl isr_ad_sample_current2: ; ; are we including some zero samples? ; movf v_smplnz_cntl,w iorwf v_smplnz_cnth,w btfss STATUS,Z bsf b_smplc_zero ; yes ; ; increment sample time ; add in smplnz_cnt (and clear it) ; add in smpln_cnt (and clear it) ; Also add in any zcnt time that may exist (and clear it) ; bsf STATUS,C movf v_smplnz_cntl,w addwfc v_smplc_cntl,f movf v_smplnz_cnth,w addwfc v_smplc_cnth,f bc isr_ad_sample_current_maxtime movf v_smpln_cntl,w addwf v_smplc_cntl,f movf v_smpln_cnth,w addwfc v_smplc_cnth,f bnc isr_ad_sample_current3 isr_ad_sample_current_maxtime: setf v_smplc_cnth ; clamp to 21 sec max setf v_smplc_cntl isr_ad_sample_current3: ; ; clear next and next_zero ; clrf v_smplnz_cntl clrf v_smplnz_cnth clrf v_smpln_cntl clrf v_smpln_cnth setf v_smpln_angle ; invalidate next angle ; ; is this the min/max magnitude of the sample so far? ; movf v_mag,w cpfslt v_smplc_min_mag movwf v_smplc_min_mag cpfsgt v_smplc_max_mag bra isr_ad_sample_current_newmaxmag bra isr_ad_done isr_ad_sample_current_newmaxmag: movwf v_smplc_max_mag movf v_angle,w movwf v_smplc_best_angle ; ; is this a tap? ; movlw -ACC_TH_MAG_TAP addwf v_smplc_max_mag,w bnc isr_ad_done ; no - quit ; ; is a tap - use tap retire threshhold ; movlw HIGH (-ACC_TH_TIME_RETIRE_TAP) movwf v_th_retimeh movlw LOW (-ACC_TH_TIME_RETIRE_TAP) movwf v_th_retimel bra isr_ad_done ;=========================================================================== ; we are seeing the same new angle ; isr_ad_sample_samenew: ; ; integrate new impulse ; movf v_mag,w addwf v_smpln_impl,f clrf WREG addwfc v_smpln_imph,f bnc isr_ad_sample_samenew2 setf v_smpln_imph ; clamp to max impulse setf v_smpln_impl isr_ad_sample_samenew2: ; ; increment new time ; incf v_smpln_cntl,f clrf WREG addwfc v_smpln_cnth,f bnc isr_ad_sample_samenew3 setf v_smpln_cntl ; clamp to max cnt setf v_smpln_cnth isr_ad_sample_samenew3: ; ; is this the min/max magnitude of the sample so far? ; movf v_mag,w cpfslt v_smpln_min_mag movwf v_smpln_min_mag cpfsgt v_smpln_max_mag bra isr_ad_sample_samenew3a bra isr_ad_sample_samenew4 isr_ad_sample_samenew3a: movwf v_smpln_max_mag movf v_angle,w movwf v_smpln_best_angle isr_ad_sample_samenew4: ; ; max magnitude big enough to be a tap? ; (if so save even if no time) ; movlw -ACC_TH_MAG_TAP addwf v_smpln_max_mag,w bc isr_ad_sample_sendresult ; ; seen state for a while? ; movlw LOW (-ACC_TH_TIME_SAVE) addwf v_smpln_cntl,w movlw HIGH (-ACC_TH_TIME_SAVE) addwfc v_smpln_cnth,w bnc isr_ad_done ; ; Have we seen enough impulse or magnitude? ; movlw LOW (-ACC_TH_IMP_SAVE) addwf v_smpln_impl,w movlw HIGH (-ACC_TH_IMP_SAVE) addwfc v_smpln_imph,w bc isr_ad_sample_sendresult ; ; Have we seen enough magnitude? ; movlw -ACC_TH_MAG_SAVE addwf v_smpln_max_mag,w bc isr_ad_sample_sendresult ; ; have not seen enough impulse to count this angle yet ; bra isr_ad_done ; ; We have seen the new angle long enough & enough impulse. ; Time to call it the current angle. ; isr_ad_sample_sendresult: bsf b_acc_motion ; ; is there a current angle to send? ; btfsc v_smplc_angle,7 bra isr_ad_sample_newcurrent ; nope ; ; is there a previous zero-time to send? ; movf v_smplpz_cnth,w iorwf v_smplpz_cntl,w bz isr_ad_sample_sendc ; nope ; ; send pause ; 0 time (h) ; 1 time (l) ; 2 unused (0xff) ; 3 angle (0xff indicates pause) ; isr_ad_sample_sendz: rcall isr_ad_sample_pre_save movf v_smplpz_cnth,w movwf POSTINC2 ; time (h) movf v_smplpz_cntl,w movwf POSTINC2 ; time (l) setf POSTINC2 ; unused (0xff) setf POSTINC2 ; angle = 0xff ; ; pause always followed by actual angle - fall thru to send angle ; ; ; send current ; 0 time (h) ; 1 time (l) ; 2 max mag ; 3 angle (0xff indicates pause) ; 4 impulse (h) ; 5 impulse (l) ; 6 min mag ; 7 flags (see below) ; ; valid values for angle ; 0x00 - 0x1f = angle ; 0xff = pause ; 0xfe = end of buffer ; ; flag bits ; 0 set if any zero samples occurred ; 1-6 unused (0) ; 7 must be set (required by isr_ad_sample_pre_save) ; isr_ad_sample_sendc: rcall isr_ad_sample_pre_save movf v_smplc_cnth,w movwf POSTINC2 ; time (h) movf v_smplc_cntl,w movwf POSTINC2 ; time (l) movf v_smplc_max_mag,w movwf POSTINC2 ; max mag movf v_smplc_best_angle,w movwf POSTINC2 ; angle movf v_smplc_imph,w movwf POSTINC2 ; impulse (h) movf v_smplc_impl,w movwf POSTINC2 ; impulse (l) movf v_smplc_min_mag,w movwf POSTINC2 ; min mag movf v_smplc_flags,w movwf POSTINC2 ; flags ;=========================================================================== ; copy smplnz and smpln to smplpz and smplc ; isr_ad_sample_newcurrent: ; ; init flags ; movlw SMPLC_FLAGS_INIT movwf v_smplc_flags ; ; reset retire time threshhold ; movlw HIGH (-ACC_TH_TIME_RETIRE) movwf v_th_retimeh movlw LOW (-ACC_TH_TIME_RETIRE) movwf v_th_retimel ; ; is smplnz_cnt big enough to count as a pause? ; movlw LOW (-ACC_TH_TIME_PAUSE) addwf v_smplnz_cntl,w movlw HIGH (-ACC_TH_TIME_PAUSE) addwfc v_smplnz_cnth,w bc isr_ad_sample_newcurrent2 ; ; no pause - incorporate smplnz time into smpln ; ; ; are we including some zero samples? ; movf v_smplnz_cntl,w iorwf v_smplnz_cnth,w btfss STATUS,Z bsf b_smplc_zero ; yes ; ; incorporate smplnz time into smpln ; movf v_smplnz_cntl,w addwf v_smpln_cntl,f movf v_smplnz_cnth,w clrf v_smplnz_cnth clrf v_smplnz_cntl addwfc v_smpln_cnth,f bnc isr_ad_sample_newcurrent2 setf v_smpln_cnth ; clamp to max setf v_smpln_cntl isr_ad_sample_newcurrent2: movff v_smplnz_cntl,v_smplpz_cntl movff v_smplnz_cnth,v_smplpz_cnth movff v_smpln_cnth,v_smplc_cnth movff v_smpln_cntl,v_smplc_cntl movff v_smpln_imph,v_smplc_imph movff v_smpln_impl,v_smplc_impl movff v_smpln_angle,v_smplc_angle movff v_smpln_best_angle,v_smplc_best_angle movff v_smpln_min_mag,v_smplc_min_mag movff v_smpln_max_mag,v_smplc_max_mag clrf v_smplnz_cntl clrf v_smplnz_cnth clrf v_smpln_cntl clrf v_smpln_cnth setf v_smpln_angle isr_ad_done: btfss b_adif return ERR ERROR_B1_ISR_OVERRUN return ;=========================================================================== ; Ensure there are (at least) 8 entries (32 bytes) in the sample buffer ; isr_ad_sample_pre_save: ; ; check for end of sample buffer ; movlw LOW (-(RAMBUF_ACC_END-32)) addwf FSR2L,w movlw HIGH (-(RAMBUF_ACC_END-32)) addwfc FSR2H,w bnc isr_ad_sample_pre_save_ok ; ; reached end of buffer - set FSR2 and disable sampling ; lfsr FSR2,(RAMBUF_ACC_END-32-1) ; ; if last entry is first half of an angle entry then change it to a pause ; High bit is only clear if it is an angle. ; Could also be a flags entry, but that always has high bit set. ; btfss INDF2,7 setf INDF2 movf POSTINC2,w ; point to 4th to last entry ; ; disable sampling? ; #if QUIT_WHEN_SAMPLE_FULL bcf b_accbuf_sample #endif isr_ad_sample_pre_save_ok: return ;=========================================================================== ; subroutine to calculate v_mag = sqrt(v_magsq) ; At this point ; v_magsql, v_magsqh = magnitude squared ; ; Result is v_mag ; ; ; v_mag is the exponent of the magnitude ; v_magsqh,l is the input ; ; isr_calc_mag: clrf v_mag nop movf v_magsqh,w bnz isr_calc_mag_4 ; high byte is 0 - shift left 8 bits (sqrt shift 4 bits) movf v_magsql,w bz isr_calc_mag_done ; sq==0 so v_mag=0 movwf v_magsqh bsf v_mag,2 ; v_mag = 4 clrf v_magsql isr_calc_mag_4: movf v_magsqh,w andlw 0xf0 bnz isr_calc_mag_2 ; high nibble is 0 - shift left 4 bits (sqrt shift 2 bits) incf v_mag,f incf v_mag,f swapf v_magsqh,w andlw 0xf0 movwf v_magsqh swapf v_magsql,w andlw 0x0f iorwf v_magsqh,f swapf v_magsql,w ; low 4 bits are garbage now isr_calc_mag_2: movf v_magsqh,w andlw 0xc0 bnz isr_calc_mag_1 ; high 2 bits are 0 - shift left 2 bits (sqrt shift 1 bit) incf v_mag,f rlcf v_magsql,f rlcf v_magsqh,f rlcf v_magsql,f ; v_magsql is mostly garbage now rlcf v_magsqh,f isr_calc_mag_1: incf v_mag,f ; loop needs cnt + 1 ; ; round up (+0x02) ; decrement high 2 bits (-0x40) ; (High 2 bits will always be 01, 10, or 11) ; movlw 0x02-0x40 addwf v_magsqh,f rrncf v_magsqh,w rrncf WREG,w andlw 0x3f ; ; save TBLPTR & TABLAT regs in v_magsq and v_isr_save_tablat ; v_isr_save_tablat equ vaddr+0 vaddr+=1 movff TBLPTRL,v_magsql movff TBLPTRH,v_magsqh movff TABLAT,v_isr_save_tablat addlw LOW (w_sqrt_table) movwf TBLPTRL movlw HIGH (w_sqrt_table) addwfc TBLPTRU,w ; add 0 + c (TBLPTRU is always 0) movwf TBLPTRH tblrd* movf TABLAT,w bnz isr_calc_mag_loop_start ; ; special case when table lookup is 0 (actual value should be 0x100) ; movlw 0x80 movwf TABLAT decfsz v_mag,f bra isr_calc_mag_loop_start ; ; special case when maxed out - just set result to 0xff ; setf v_mag return isr_calc_mag_loop: bcf TABLAT,0 rrncf TABLAT,f isr_calc_mag_loop_start: decfsz v_mag,f bra isr_calc_mag_loop movff TABLAT,v_mag ; ; restore TBLPTR & TABLAT regs from v_magsq and v_isr_save_tablat ; movff v_magsql,TBLPTRL movff v_magsqh,TBLPTRH movff v_isr_save_tablat,TABLAT isr_calc_mag_done: return ; ; sqrt table, calculated by my_sqrt16.c ; w_sqrt_table: db 0x80, 0x83, 0x87, 0x8b, 0x8f, 0x92, 0x96, 0x99 db 0x9c, 0xa0, 0xa3, 0xa6, 0xa9, 0xac, 0xaf, 0xb2 db 0xb5, 0xb7, 0xba, 0xbd, 0xc0, 0xc2, 0xc5, 0xc7 db 0xca, 0xcc, 0xcf, 0xd1, 0xd4, 0xd6, 0xd9, 0xdb db 0xdd, 0xe0, 0xe2, 0xe4, 0xe6, 0xe8, 0xeb, 0xed db 0xef, 0xf1, 0xf3, 0xf5, 0xf7, 0xf9, 0xfb, 0xfe db 0x00 ;=========================================================================== ; subroutine to calculate angle ; At this point ; v_absx = abs val of x ; v_absy = abs val of y ; v_angle = ; 0 if x >= 0 && y >= 0 ; -15 if x >= 0 && y < 0 ; -31 if x < 0 && y >= 0 ; 16 if x < 0 && y < 0 ; isr_calc_angle: movf v_absx,w subwf v_absy,w bnn isr_ad_angle2 ; if absx <= absy skip this ; ; absx > absy ; movlw 7 ; angle = -(angle+7) addwf v_angle,f negf v_angle movf v_absy,w ; swap absx <-> absy movff v_absx,v_absy movwf v_absx isr_ad_angle2: movf v_absx,w addwf v_absx,w ; w = absx * 2 movwf v_absx addwf v_absx,f ; v_absx = absx * 4 ; ; compare 2*absx >? absy ; subwf v_absy,w bnn isr_ad_angle3 ; if 2*absx <= absy skip this ; ; 2*absx > absy ; movlw 3 ; angle = -(angle+3) addwf v_angle,f negf v_angle movf v_absy,w ; w = 3*absy addwf v_absy,w addwf v_absy,w movff v_absx,v_absy ; swap 4*absx and 3*absy movwf v_absx isr_ad_angle3: ; ; do one of these comparisons: ; 4*absx >? 1*absy ; 3*absy >? 4*absx ; movf v_absx,w ; 4*absx or 3*absy subwf v_absy,w ; absy or 4*absx btfsc STATUS,N incf v_angle,f ; v_absx > v_absy - increment angle ; ; absolute value is resulting angle ; btfsc v_angle,7 negf v_angle ; ; compensate for 45 degree rotation of sensor ; movf v_angle,w addlw -4 andlw 0x1f movwf v_angle return #if DBORG org 0x0f00 #endif ;########################################################################### ;################################ DEBUG FUNCTIONS ########################## ;########################################################################### ; ; Macros: ; ; DB_BYTE ch,reg print char and register (save & restore w & other regs) ; DB_FLUSH flush tx buffer ; DB_BYTE macro ch,reg movwf v_dbg_save_w movff reg,v_dbg_low movlw ch call w_dbg_byte endm DB_FLUSH macro call w_dbg_flush endm v_dbg_low equ vaddr+0 ; v_dbg_ch equ vaddr+1 ; v_dbg_save_w equ vaddr+2 ; v_dbg_save_fsr0l equ vaddr+3 ; v_dbg_save_fsr0h equ vaddr+4 ; vaddr+=5 ; ; Functions: ; ; w_dbg_byte - print a byte with a char ; w_dbg_flush - flush tx buffer ; ; ; w_dbg_byte - print a byte with a char ; v_dbg_low - byte ; v_dbg_char - character ; w_dbg_byte: btfss RCSTA,SPEN ; do not bother to print if serial disabled return movff FSR0L,v_dbg_save_fsr0l movff FSR0H,v_dbg_save_fsr0h call kk_cout COUTL '=' movf v_dbg_low,w call kk_outbyte COUTL ' ' w_dbg_return: movff v_dbg_save_fsr0l,FSR0L movff v_dbg_save_fsr0h,FSR0H movf v_dbg_save_w,w return w_dbg_flush: btfsc RCSTA,SPEN ; do not bother to print if serial disabled btfsc bk_rs_halt return call w_main_run movf vk_tx_cnt,w bnz w_dbg_flush return ;########################################################################### ;################################ A/D FUNCTIONS ############################ ;########################################################################### ; ; w_ad_init - turn on the accelerometer for non-ISR sampling ; w_ad_start - turn on the accelerometer ; w_ad_stop - turn off the accelerometer ; w_ad_sample_finish - turn off accelerometer and flush sample output ; w_sample_start - start sampling into buffer (call after w_ad_start) ; w_sample_clear - clear sample buffer to 0 ; ; #if ENAB_ISR_TAP ; w_tap_enable - clear and enable taps (call after w_ad_start) #endif ;=========================================================================== ; w_ad_start - turn on the accelerometer & start ISR ;=========================================================================== w_ad_start: ; ; initialize accelerometer & A/D conversion ; rcall w_ad_init ; ; setup auto-sampling and ISR ; clrf CCPR2H movlw REGVAL_CCPRL2L movwf CCPR2L movlw REGVAL_CCP2CON movwf CCP2CON bsf b_adip ; a/d interrupt is high priority bsf b_adie ; a/d interrupt is enabled movlw REGVAL_T3CON_AD movwf T3CON ; start timer3 return ;=========================================================================== ; w_ad_init - turn on the accelerometer (no ISR) ;=========================================================================== w_ad_init: ; ; disable isr while we set things up ; bcf b_gieh ; disable high priority interrupts ; ; stop ongoing conversion (if any) ; rcall w_ad_stop ; ; clear timer3 ; clrf TMR3H clrf TMR3L ; ; wait until accelerometer settles ; call w_pause_ad_settle ; ; disable sampling ; bcf b_accbuf_sample ; ; setup min/max values ; setf v_accx_min setf v_accy_min clrf v_accx_max clrf v_accy_max clrf v_accx clrf v_accy ; ; init astate ; clrf v_angle clrf v_mag #if ENAB_ISR_TAP ; ; disable taps ; bcf b_tap bcf b_tap_mask bcf b_tap_enable #endif ; ; point FSR2 at acceleration buffer ; lfsr FSR2,RAMBUF_ACC ; ; turn on accelerometer ; bcf b_acc_disable ; ; set up timing ; movlw KERNVAL_ADCON2 movwf ADCON2 ; ; set up to sample x axis ; movlw ADCON0_ACCX movwf ADCON0 bsf b_do_accx ; start with x axis ; ; disable Capture Compare (auto-triggers accelerometer) ; clrf CCP2CON ; ; disable a/d interrupt ; bcf b_adie ; a/d interrupt is disabled bcf b_adif ; clear any old pending interrupt ; ; re-enable high priority interrupts ; bsf b_gieh ; enable high priority interrupts return ;=========================================================================== ; w_ad_sample_finish - turn off accelerometer and flush sample output ;=========================================================================== w_ad_sample_finish: rcall w_ad_stop rcall isr_ad_sample_sendresult rcall isr_ad_sample_sendresult ; ; mark end of buffer with special sample ; (save 2 copies in case first is 2nd half of angle sample) ; rcall isr_ad_sample_pre_save ; ; long pause ; clrf POSTINC2 ; time(h) = 0 setf POSTINC2 ; time(l) = 0xff setf POSTINC2 ; unused setf POSTINC2 ; angle = 0xff (pause) ; ; mark end of buffer ; clrf POSTINC2 setf POSTINC2 setf POSTINC2 movlw 0xfe movwf POSTINC2 ; 0xfe in angle marks end of samples return ;=========================================================================== ; w_ad_stop - turn off the accelerometer ;=========================================================================== w_ad_stop: bcf b_adie ; a/d interrupt is disabled clrf T3CON ; stop timer3 btfsc b_adgo bra w_ad_stop ; wait until any a/d conversion completes clrf ADCON0 ; a/d unit off bsf b_acc_disable ; disable accelerometer power return #if ENAB_ISR_TAP ;=========================================================================== ; w_tap_enable - clear and enable taps ;=========================================================================== w_tap_enable: bcf b_tap bcf b_tap_mask bsf b_tap_enable clrf v_tap_bits return #endif ;=========================================================================== ; w_sample_start - begin storing samples into buffer ; (call w_ad_start first) ;=========================================================================== w_sample_start: bcf b_accbuf_sample ; disable sampling bcf b_acc_finish ; disable finish request lfsr FSR2, RAMBUF_ACC ; ; init astate buffering ; clrf v_angle clrf v_mag #if USE_CONST_MAGSQ_TH movlw HIGH (-ACC_INIT_TH_MAGSQ_Z) movwf v_th_magsqh movlw LOW (-ACC_INIT_TH_MAGSQ_Z) movwf v_th_magsql #else movff v_th_magsql_z,v_th_magsql #endif movlw HIGH (-ACC_TH_TIME_RETIRE) movwf v_th_retimeh movlw LOW (-ACC_TH_TIME_RETIRE) movwf v_th_retimel ; ; init sampling state vars ; setf v_smplc_angle ; we have no sample angle setf v_smpln_angle ; we have no new sample angle clrf v_smpln_cntl ; no new sample time clrf v_smpln_cnth clrf v_smplnz_cntl ; no new zero sample time clrf v_smplnz_cnth #if ENAB_W_PARSE ; ; store initial sample in buffer ; rcall w_sample_store_zero #endif bsf b_accbuf_sample ; enable sampling return #if ENAB_W_PARSE ;=========================================================================== ; w_sample_store_zero - store a zero-entry in acc buffer ; call with b_accbuf_sample not set ;=========================================================================== w_sample_store_zero: rcall isr_ad_sample_pre_save setf POSTINC2 ; time (l) max setf POSTINC2 ; time (h) max setf POSTINC2 ; time (h) max setf POSTINC2 ; angle - none return #endif ;=========================================================================== ; w_sample_clear - clear sample & motion buffer to 0 ;=========================================================================== w_sample_clear: lfsr FSR0,RAMBUF_ACC w_sample_clear_loop: clrf POSTINC0 movlw LOW RAMBUF_ACC_END subwf FSR0L,w movlw HIGH RAMBUF_ACC_END subwfb FSR0H,w bnc w_sample_clear_loop return #if DBORG org 0x1100 #endif ;########################################################################### ;################################ STRINGS ################################## ;########################################################################### #include "wstrings.inc" w_sout_base1: addlw LOW w_str_base1 movwf TBLPTRL movlw HIGH w_str_base1 addwfc TBLPTRU,w ; add 0 + c (TBLPTRU is always 0) ; fall thru w_sout_wh: btfss RCSTA,SPEN ; do not bother to print if serial disabled return goto kk_sout_wh w_outbyte: btfss RCSTA,SPEN ; do not bother to print if serial disabled return goto kk_outbyte w_outbyte_sp: btfss RCSTA,SPEN ; do not bother to print if serial disabled return call kk_outbyte movlw ' ' goto kk_cout w_sout_base1_cr: btfss RCSTA,SPEN ; do not bother to print if serial disabled return rcall w_sout_base1 ; fall thru w_outcr: btfss RCSTA,SPEN ; do not bother to print if serial disabled return call kk_outcr bra w_dbg_flush ;########################################################################### ;################################ LED BLINKING ############################# ;########################################################################### #if ENAB_BLINKS ; ; START_BLINK macro begins a blink sequence ; START_BLINK macro addr movlw addr-w_blinkpatterns+1 movwf v_blink_ptr bsf b_blink endm w_blinkpatterns: w_blinkpat_left: ; (down-up) db 0xfb, 0x04 db 0xff, 0x5f db 0xf7, 0x04 db 0xff, 0x5f db 0xef, 0x04 db 0xff, 0x5f db 0xdf, 0x04 db 0xff, 0x5f db 0xbf, 0x04 db 0xff, 0x5f w_blinkpat_up: db 0x7f, 0x04 db 0xff, 0x5f db 0xbf, 0x04 db 0xff, 0x5f db 0xdf, 0x04 db 0xff, 0x5f db 0xef, 0x04 db 0xff, 0x5f db 0xf7, 0x04 db 0xff, 0x5f db 0xfb, 0x04 db 0x00, 0x00 w_blinkpat_right: ; (up-down) db 0x7f, 0x04 db 0xff, 0x5f db 0xbf, 0x04 db 0xff, 0x5f db 0xdf, 0x04 db 0xff, 0x5f db 0xef, 0x04 db 0xff, 0x5f db 0xf7, 0x04 db 0xff, 0x5f w_blinkpat_down: db 0xfb, 0x04 db 0xff, 0x5f db 0xf7, 0x04 db 0xff, 0x5f db 0xef, 0x04 db 0xff, 0x5f db 0xdf, 0x04 db 0xff, 0x5f db 0xbf, 0x04 db 0xff, 0x5f db 0x7f, 0x04 db 0x00, 0x00 w_blinkpat_countdown: db 0xff, 0x01 db 0x03, 0x04 db 0xff, 0xff db 0x07, 0x04 db 0xff, 0xff db 0x0f, 0x04 db 0xff, 0xff db 0x1f, 0x04 db 0xff, 0xff db 0x3f, 0x04 db 0xff, 0xff db 0x7f, 0x04 db 0x00, 0x00 w_blinkpat_tip: db 0xff, 0xff db 0xfb, 0x04 db 0x00, 0x00 ; ; Call w_blink every main loop to handle blinking. ; Aceleration interrupt must be on (it implements the timer) ; w_blink: movf v_blink_ptr,w ; skip if no sequence is going bz w_blink_end addlw LOW (w_blinkpatterns-1) movwf TBLPTRL movlw HIGH (w_blinkpatterns-1) addwfc TBLPTRU,w ; add 0 + c (TBLPTRU is always 0) movwf TBLPTRH movlw KERNVAL_TRISB_LED_MASK iorwf TRISB,f incf v_blink_ptr,f incf v_blink_ptr,f tblrd*+ movf TABLAT,w bz w_blink_finish andwf TRISB,f tblrd*+ movff TABLAT,v_blink_cnt w_blink_end: bcf b_blink return w_blink_finish: clrf v_blink_ptr bra w_blink_end #endif ;########################################################################### ;################################ HANDLE TAPS ############################## ;########################################################################### #if ENAB_OLD_TAP_HANDLE ; ; Handle taps (call when b_tap is set) ; w_tap: #if DBPRINT ; ; debug - print tap ; COUTL 'T' COUTL 'a' COUTL 'p' movf v_tap_angle,w call kk_outbyte call kk_outcr #endif ; ; clear tap bit ; bcf b_tap ; ; round tap angle to quadrant ; movf v_tap_angle,w addlw 4 rrncf WREG,w rrncf WREG,w rrncf WREG,w andlw 0x03 ; ; up tap? ; w_tap_up: bnz w_tap_right #if ENAB_BLINKS START_BLINK w_blinkpat_up #endif bsf b_tap_up return ; ; right tap? ; w_tap_right: decfsz WREG,w bra w_tap_down #if 0 #if ENAB_BLINKS START_BLINK w_blinkpat_right #endif #endif bsf b_tap_right return ; ; down tap? ; w_tap_down: decfsz WREG,w bra w_tap_left #if ENAB_BLINKS START_BLINK w_blinkpat_down #endif bsf b_tap_down return ; ; left tap? ; w_tap_left: #if 0 #if ENAB_BLINKS START_BLINK w_blinkpat_left #endif #endif bsf b_tap_left return #endif ;########################################################################### ;################################ DRAW CHARACTERS ########################## ;########################################################################### #if DBORG org 0x1300 #endif ; ; DB_DRAW_ABCD - print a,b,c,d when state transition occurs ; DB_DRAW_LED - indicate state on LEDs ; DB_DRAW_TAP - print when tap occurs ; DB_DRAW_SLEEP - print when we need to sleep ; DB_DRAW_STOP - print when stopping ; DB_DRAW_TIME - print min/max time ; DB_DRAW_WHERE - record where we are in program ; #define DB_DRAW_ABCD 0 #define DB_DRAW_LED 0 #define DB_DRAW_TAP 1 #define DB_DRAW_SLEEP 1 #define DB_DRAW_STOP 1 #define DB_DRAW_TIME 0 #define DB_DRAW_WHERE 0 #if DB_DRAW_WHERE v_draw_where1 equ vaddr+0 v_draw_where2 equ vaddr+1 vaddr+=2 DB_DWHERE1 macro val movlw val movwf v_draw_where1 endm DB_DWHERE2 macro val movlw val movwf v_draw_where2 endm #else DB_DWHERE1 macro val endm DB_DWHERE2 macro val endm #endif v_draw_val equ vaddr+0 ; pixel value for current column v_accs equ vaddr+1 ; v_accx - v_accy v_maxs equ vaddr+2 ; max value of v_accs (or threshhold) v_mins equ vaddr+3 ; min value of v_accs (or threshhold) v_last_maxs equ vaddr+4 ; previous max value of v_accs v_last_mins equ vaddr+5 ; previous min value of v_accs vaddr+=6 v_tdrawl equ vaddr+0 ; time when drawing should commence v_tdrawh equ vaddr+1 ; v_tendl equ vaddr+2 ; time when drawing should commence v_tendh equ vaddr+3 ; vaddr+=4 v_zcnt equ vaddr+0 ; how long since magnitude was near 0 (for tap) vaddr+=1 v_tapx equ vaddr+0 ; x and y from tap v_tapy equ vaddr+1 ; vaddr+=2 ; ; all LEDs off ; accelerometer on ; #if HW_VERSION>=2 DRAW2_TRISB_OFF_VAL equ KERNVAL_TRISB_LED_MASK #else DRAW2_TRISB_OFF_VAL equ 0xfd #endif ; ; begin draw sequence ; w_draw2: ; ; check for scrolling (long message) ; rcall w_wscrollcheck call w_ad_init clrf v_accx clrf v_accy movlw DRAW2_TRISB_OFF_VAL movwf v_draw_val ; ; enable brightest LEDS ; bcf b_maxbright_disable bcf b_medbright_disable ; ; wait before sleeping ; allow taps immediately ; movlw DRAW2_SLEEP_WAIT movwf v_sleep_wait setf v_tap_wait ; ; tap stuff ; setf v_zcnt bcf |