// // fmtb_pic.c - This file is the pic code for fmtb_parse.c // // 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 // #ifndef PIC_CSIM #define PIC_CSIM 0 #endif #define ALLOW_GAPS_IN_STRONG_CIRCLES 1 #define FIX_CIRCLE_OCT_COMPARE 1 #define MAX_TOKEN_CNT 256 #define USE_LEDSCAN 1 // // See wand.asm MEMORY MAP for details // picEqu(RAMBUF_MTOKENS, 0xc00) picEqu(RAMBUF_MTOKENS_LEN, 0x100) picEqu(RAMBUF_M2LIST, 0x300) picEqu(RAMBUF_M2LIST_LEN, 0x500) picEqu(RAMBUF_M3LIST, 0x800) picEqu(RAMBUF_M3LIST_LEN, 0x500) picEqu(RAMBUF_ACC, 0x500) picEqu(RAMBUF_ACC_LEN, 0xa00) picEqu(RAMBUF_STOKENS, 0xb00) picEqu(RAMBUF_STOKENS_LEN, 0x100) picEqu(RAMBUF_SPELLDATA, 0x900) picEqu(RAMBUF_SPELLDATA_LEN, 0x100) picEqu(RAMBUF_SD_SPELL_IGNORE, 0x900) picEqu(RAMBUF_SD_MOTION_IGNORE, 0x908) #if !USE_LEDSCAN picEqu(RAMBUF_SD_LED, 0x910) #endif picEqu(RAMBUF_MMATRIX, 0xa00) picEqu(RAMBUF_MMATRIX_LEN, 0x100) picEqu(RAMBUF_STBL1, 0x800) picEqu(RAMBUF_STBL1_LEN, 0x100) picEqu(RAMBUF_STBL2, 0x700) picEqu(RAMBUF_STBL2_LEN, 0x100) //=========================================================================== // declare global registers //=========================================================================== #if PIC_CSIM static void init_pic_globals(void) #endif { // // common variables // picRegGlobal(v_mo_angle, "fmtb"); picRegGlobal(v_mo_oct, "fmtb"); picRegGlobal(v_mo_impl, "fmtb"); picRegGlobal(v_mo_imph, "fmtb"); picRegGlobal(v_mo_maxmag, "fmtb"); picRegGlobal(v_mo_shape, "fmtb"); picRegGlobal(v_mo_save_fsr0l, "fmtb"); picRegGlobal(v_mo_save_fsr0h, "fmtb"); picRegGlobal(v_mo_save_fsr1l, "fmtb"); picRegGlobal(v_mo_save_fsr1h, "fmtb"); picRegGlobal(v_mo_save_fsr2l, "fmtb"); picRegGlobal(v_mo_save_fsr2h, "fmtb"); // // local variables for mo1 // picRegGlobal(v_mo1_mo_angle, "mo1"); picRegGlobal(v_mo1_mo_maxmag, "mo1"); picRegGlobal(v_mo1_mo_minmag, "mo1"); picRegGlobal(v_mo1_mo_impl, "mo1"); picRegGlobal(v_mo1_mo_imph, "mo1"); picRegGlobal(v_mo1_mo_flags, "mo1"); // // locals for mo2 // picRegGlobal(v_mo2_next_mh, "mo2"); picRegGlobal(v_mo2_next_ml, "mo2"); picRegGlobal(v_mo2_next_mdh, "mo2"); picRegGlobal(v_mo2_next_mdl, "mo2"); picRegGlobal(v_mo2_bits, "mo2"); #define PICDST_BITb_MSHF_PLUS PICDST_BIT0 #define PICDST_BITb_MSHF_ZERO PICDST_BIT0 #define PICDST_BITb_MSHF_DIR PICDST_BIT3 #define PICDST_BITb_MSHF_CIRCLE PICDST_BIT5 #define PICDST_BITb_MSHF_MONO PICDST_BIT6 #define PICDST_BITb_MSHF_PREV_PAUSE PICDST_BIT7 ASSERT(MSHF_ZERO == (1<<(PICDST_BITb_MSHF_ZERO-PICDST_BIT0))); ASSERT(MSHF_PLUS == (1<<(PICDST_BITb_MSHF_PLUS-PICDST_BIT0))); ASSERT(MSHF_DIR == (1<<(PICDST_BITb_MSHF_DIR-PICDST_BIT0))); ASSERT(MSHF_CIRCLE == (1<<(PICDST_BITb_MSHF_CIRCLE-PICDST_BIT0))); ASSERT(MSHF_PREV_PAUSE == (1<<(PICDST_BITb_MSHF_PREV_PAUSE-PICDST_BIT0))); ASSERT(MSHF_MONO == (1<<(PICDST_BITb_MSHF_MONO-PICDST_BIT0))); #define PICDST_BITb_TOK_DIR PICDST_BIT7 ASSERT(TOK_DIR == (1<<(PICDST_BITb_TOK_DIR-PICDST_BIT0))); #define PICDST_BITb_AFLG_END PICDST_BIT6 #define PICDST_BITb_AFLG_START PICDST_BIT5 ASSERT(AFLG_END == (1<<(PICDST_BITb_AFLG_END-PICDST_BIT0))); ASSERT(AFLG_START == (1<<(PICDST_BITb_AFLG_START-PICDST_BIT0))); #define PICDST_BITb_MWN_PULSE PICDST_BIT7 #define PICDST_BITb_MWN_DIR PICDST_BIT6 #define PICDST_BITb_MWN_MORE PICDST_BIT5 ASSERT(MWN_PULSE == (1<<(PICDST_BITb_MWN_PULSE-PICDST_BIT0))); ASSERT(MWN_DIR == (1<<(PICDST_BITb_MWN_DIR-PICDST_BIT0))); ASSERT(MWN_MORE == (1<<(PICDST_BITb_MWN_MORE-PICDST_BIT0))); } //=========================================================================== // w_a2oct: convert angle (in W) to octant (in W) //=========================================================================== static void w_a2oct(void) { addlw(2); rrncf(WREG,w); rrncf(WREG,w); andlw(7); picReturn(); } //=========================================================================== // w_oct2a: convert octant (in W) to angle (in W) //=========================================================================== static void w_oct2a(void) { rlncf(WREG,w); rlncf(WREG,w); andlw(0x1f); picReturn(); } //=========================================================================== // w_adiff: convert angle a1-a0 (in W) to range -16 ... 15 //=========================================================================== static void w_adiff(void) { andlw(0x1f); btfsc(WREG,4); addlw(-0x20); picReturn(); } //=========================================================================== // w_odiff: convert octant o1-o0 (in W) to range -4 ... 3 //=========================================================================== static void w_odiff(void) { andlw(0x07); btfsc(WREG,2); addlw(-0x08); picReturn(); } #if 0 //=========================================================================== // w_a2quad: convert angle (in W) to quad (in W) //=========================================================================== static void w_a2quad(void) { addlw(4); rrncf(WREG,w); rrncf(WREG,w); rrncf(WREG,w); andlw(3); picReturn(); } #endif //=========================================================================== // w_mo_read: //=========================================================================== static void w_mo_read(void) { // // Fills in these registers: // v_mo1_mo_angle // v_mo1_mo_maxmag // v_mo1_mo_minmag // v_mo1_mo_impl // v_mo1_mo_imph // v_mo1_mo_flags // // Read as follows from FSR2: // // Pause: // 0 - time (h) // 1 - time (l) // 2 - unused (set to 0xff) // 3 - angle (0xff indicates pause) // // Angle: // 0 - time (h) // 1 - time (l) // 2 - max mag // 3 - angle (0-31) // 4 - imp (h) // 5 - imp (l) // 6 - min mag // 7 - flags // movf(POSTINC2,w); // timeh movf(POSTINC2,w); // timel movf(POSTINC2,w); movwf(v_mo1_mo_maxmag); movf(POSTINC2,w); movwf(v_mo1_mo_angle); btfsc(v_mo1_mo_angle,7); picReturn(); movf(POSTINC2,w); movwf(v_mo1_mo_imph); // imph movf(POSTINC2,w); movwf(v_mo1_mo_impl); // impl movf(POSTINC2,w); movwf(v_mo1_mo_minmag); // minmag movf(POSTINC2,w); movwf(v_mo1_mo_flags); // flags (only bit0 counts) picReturn(); } //=========================================================================== // w_shp_read: read from FSR2 //=========================================================================== static void w_shp_read(void) { // // read Motion2 from FSR2 // movf(POSTINC2,w); movwf(v_mo_angle); movf(POSTINC2,w); movwf(v_mo_impl); movf(POSTINC2,w); movwf(v_mo_imph); movf(POSTINC2,w); movwf(v_mo_maxmag); movf(POSTINC2,w); movwf(v_mo_shape); picReturn(); ASSERT(0); } //=========================================================================== // w_shp_write: write to FSR0 //=========================================================================== static void w_shp_write(void) { // // write Motion2 to FSR0 // movf(v_mo_angle,w); movwf(POSTINC0); movf(v_mo_impl,w); movwf(POSTINC0); movf(v_mo_imph,w); movwf(POSTINC0); movf(v_mo_maxmag,w); movwf(POSTINC0); movf(v_mo_shape,w); movwf(POSTINC0); picReturn(); ASSERT(0); } //=========================================================================== // w_shp_prev: - back up to previous motion2 record //=========================================================================== static void w_shp_prev(void) { // // back up to previous motion2 record // movlw(5); subwf(FSR2L,f); movlw(0); subwfb(FSR2H,f); picReturn(); ASSERT(0); } //=========================================================================== // w_mo1_parse1: //=========================================================================== static void w_mo1_parse1(void) { #if PIC_CSIM int dbg = DBG_PARSE1_MINIMAL; int oldSilent = setSilent(OPT_silent); int oldTrace; #endif // // local variables // int picRegLocal(v_mo1_bits); #define PICDST_BITb_mo1_was_pause PICDST_BIT0 #define PICDST_BITb_mo1_got_angle PICDST_BIT1 #define PICDST_BITb_mo1_got_dir PICDST_BIT2 int picRegLocal(v_mo1_dir); int picRegLocal(v_mo1_shape_flags); int picRegLocal(v_mo1_prev_angle); int picRegLocal(v_mo1_prev_oct); int picRegLocal(v_mo1_step); int picRegLocal(v_mo1_maxdmag); int picRegLocal(v_mo1_maxstep); int picRegLocal(v_mo1_asum); #if PIC_CSIM if (dbg) { oldSilent = setSilent(0); oldTrace = picTrace(1); } else { oldTrace = picTrace(0); } #endif lfsr(FSR2,RAMBUF_ACC); lfsr(FSR0,RAMBUF_M2LIST); // // first motion is STARTT // movlw(ANGLE_START); movwf(v_mo1_mo_angle); movwf(POSTINC0); // angle clrf(POSTINC0); // impl clrf(POSTINC0); // imph clrf(POSTINC0); // maxmag clrf(POSTINC0); // shape clrf(v_mo1_dir); clrf(v_mo1_bits); bsf(v_mo1_bits,b_mo1_was_pause); // // these probably do not need to be cleared // clrf(v_mo1_shape_flags); picLabel(w_mo1_outer_loop); movf(v_mo1_mo_angle,w); movwf(v_mo1_prev_angle); movf(v_mo_oct,w); movwf(v_mo1_prev_oct); clrf(v_mo1_step); rcall(w_mo_read); #if PIC_CSIM if (dbg) { Motion m; memset(&m,0,sizeof(m)); m.b_angle = picRV(v_mo1_mo_angle); m.b_maxmag = picRV(v_mo1_mo_maxmag); m.b_imp = picRV(v_mo1_mo_impl) + (picRV(v_mo1_mo_imph) << 8); m.b_minmag = picRV(v_mo1_mo_minmag); m.b_flags = picRV(v_mo1_mo_flags); printMotion("READ1 ",&m,&m,2); breakpt(); } #endif movf(v_mo1_mo_angle,w); rcall(w_a2oct); movwf(v_mo_oct); // // is it pause or end of buffer? // btfss(v_mo1_mo_angle,7); bra(w_mo1_top_checkpause); // // restart new shape if this is a new pause // btfss(v_mo1_bits,b_mo1_was_pause); bra(w_mo1_restart); // // repeated pause? (not end of buffer?) - loop // btfsc(v_mo1_mo_angle,0); bra(w_mo1_outer_loop); // // end of buffer - DONE // bra(w_mo1_done); picLabel(w_mo1_top_checkpause); btfss(v_mo1_bits,b_mo1_was_pause); bra(w_mo1_top_checkangle); bcf(v_mo1_bits,b_mo1_got_angle); bra(w_mo1_restart); picLabel(w_mo1_top_checkangle); // // calc v_mo1_step & v_mo1_dir // clrf(v_mo1_dir); movf(v_mo1_prev_angle,w); subwf(v_mo1_mo_angle,w); andlw(0x1f); btfss(WREG,4); // check for negative bra(w_mo1_top_checkangle2); addlw(-0x20); negf(WREG); bsf(v_mo1_dir,b_MSHF_DIR); picLabel(w_mo1_top_checkangle2); movwf(v_mo1_step); movf(v_mo_oct,w); subwf(v_mo1_prev_oct,w); bz(w_mo1_bottom); // fall through to restart picLabel(w_mo1_restart); btfss(v_mo1_bits,b_mo1_got_angle); bra(w_mo1_restart2); // // got a motion angle - send a shape // movf(v_mo1_step,w); addwf(v_mo1_asum,f); cpfsgt(v_mo1_maxstep); movwf(v_mo1_maxstep); movlw(MSHP_PULSE); movwf(v_mo_shape); // // clear MONO if no dir was found // btfss(v_mo1_bits,b_mo1_got_dir); bcf(v_mo1_shape_flags,b_MSHF_MONO); // // clear MONO if direction changed // movf(v_mo1_dir,w); xorwf(v_mo1_shape_flags,w); btfsc(WREG,b_MSHF_DIR); bcf(v_mo1_shape_flags,b_MSHF_MONO); // // if !MONO send pulse // btfss(v_mo1_shape_flags,b_MSHF_MONO); bra(w_mo1_send_shape); // // if MSHF_PREV_PAUSE send pulse // if new angle is a pause (or end of buffer) then send pulse // btfss(v_mo1_shape_flags,b_MSHF_PREV_PAUSE); btfsc(v_mo1_mo_angle,7); bra(w_mo1_send_shape); // // if asum < 4 send pulse // movf(v_mo1_asum,w); addlw(-4); bnc(w_mo1_send_shape); // // if maxstep >= 6 send pulse // movf(v_mo1_maxstep,w); addlw(-6); bc(w_mo1_send_shape); // // Not a pulse! dirty circle? // ASSERT(MSHP_DIRTY_CIRCLE == 0x00); ASSERT(MSHP_PULSE == (1<<2)); ASSERT(picRV(v_mo_shape) == MSHP_PULSE); bcf(v_mo_shape,2); ASSERT(picRV(v_mo_shape) == MSHP_DIRTY_CIRCLE); // // if maxstep >= 4 send dirty circle // movf(v_mo1_maxstep,w); addlw(-4); bc(w_mo1_send_shape); // // if maxdmag >= 8 send dirty circle // movf(v_mo1_maxdmag,w); addlw(-8); bc(w_mo1_send_shape); // // if ZERO then send dirty circle // btfsc(v_mo1_shape_flags,b_MSHF_ZERO); bra(w_mo1_send_shape); // // send clean circle // ASSERT(MSHP_CLEAN_CIRCLE == (1<<1)); ASSERT(picRV(v_mo_shape) == 0); bsf(v_mo_shape,1); ASSERT(picRV(v_mo_shape) == MSHP_CLEAN_CIRCLE); // // SEND THE SHAPE // picLabel(w_mo1_send_shape); movf(v_mo1_shape_flags,w); andlw(MSHF_FLG_MASK); iorwf(v_mo_shape,f); // // clear dir flag if !MONO // btfss(v_mo_shape,b_MSHF_MONO); bcf(v_mo_shape,b_MSHF_DIR); #if PIC_CSIM if (dbg) { Motion2 m; memset(&m,0,sizeof(m)); m.shape = picRV(v_mo_shape); m.maxmag = picRV(v_mo_maxmag); m.imp = picRV(v_mo_impl) + (picRV(v_mo_imph) << 8); m.angle = picRV(v_mo_angle); printMotion2("WRITE ",&m,&m,4); breakpt(); } #endif rcall(w_shp_write); picLabel(w_mo1_restart2); // // are we at the end? // movf(v_mo1_mo_angle,w); addlw(-0xfe); bz(w_mo1_done); clrf(v_mo1_asum); clrf(v_mo_impl); clrf(v_mo_imph); clrf(v_mo1_maxstep); clrf(v_mo1_maxdmag); clrf(v_mo_maxmag); bcf(v_mo1_bits,b_mo1_got_angle); movlw(MSHF_DIR); andwf(v_mo1_shape_flags,f); bsf(v_mo1_shape_flags,b_MSHF_MONO); // // bottom - after restart // picLabel(w_mo1_bottom); btfss(v_mo1_mo_angle,7); bra(w_mo1_bottom_notpause); // // pause // bsf(v_mo1_bits,b_mo1_was_pause); bra(w_mo1_outer_loop); picLabel(w_mo1_bottom_notpause); btfss(v_mo1_bits,b_mo1_was_pause); bra(w_mo1_bottom_angle); // // was pause last time // bcf(v_mo1_bits,b_mo1_was_pause); bcf(v_mo1_bits,b_mo1_got_dir); bsf(v_mo1_shape_flags,b_MSHF_PREV_PAUSE); if (OPT_use_classic) { bcf(v_mo1_shape_flags,b_MSHF_MONO); } bra(w_mo1_getangle); // // angle (not the first one) // picLabel(w_mo1_bottom_angle); // // if !got_dir // copy DIR bit frim mo_dir // btfsc(v_mo1_bits,b_mo1_got_dir); bra(w_mo1_checkdir); bcf(v_mo1_shape_flags,b_MSHF_DIR); movf(v_mo1_dir,w); andlw(MSHF_DIR); iorwf(v_mo1_shape_flags,f); picLabel(w_mo1_checkdir); bsf(v_mo1_bits,b_mo1_got_dir); movf(v_mo1_dir,w); xorwf(v_mo1_shape_flags,w); andlw(MSHF_DIR); bz(w_mo1_getangle); // // change in direction detected // xorwf(v_mo1_shape_flags,f); bcf(v_mo1_shape_flags,b_MSHF_MONO); picLabel(w_mo1_getangle); // // got an angle - record info from it // bsf(v_mo1_bits,b_mo1_got_angle); movf(v_mo1_mo_impl,w); addwf(v_mo_impl,f); movf(v_mo1_mo_imph,w); addwfc(v_mo_imph,f); bnc(w_mo1_impwrap); setf(v_mo_impl); setf(v_mo_imph); picLabel(w_mo1_impwrap); movf(v_mo1_mo_maxmag,w); subwf(v_mo_maxmag,w); bc(w_mo1_notmax); movf(v_mo1_mo_maxmag,w); movwf(v_mo_maxmag); movf(v_mo1_mo_angle,w); movwf(v_mo_angle); picLabel(w_mo1_notmax); ASSERT(MSHF_ZERO == MFLG_ZERO); movf(v_mo1_mo_flags, w); andlw(MSHF_ZERO); iorwf(v_mo1_shape_flags,f); // // calc dmag & save max // movf(v_mo1_mo_minmag,w); subwf(v_mo1_mo_maxmag,w); btfsc(v_mo1_mo_flags,b_MSHF_ZERO); movf(v_mo1_mo_maxmag,w); cpfsgt(v_mo1_maxdmag); movwf(v_mo1_maxdmag); movf(v_mo1_step,w); addwf(v_mo1_asum,f); cpfsgt(v_mo1_maxstep); movwf(v_mo1_maxstep); bra(w_mo1_outer_loop); // // End of loop // picLabel(w_mo1_done); movlw(ANGLE_END); movwf(v_mo_angle); rcall(w_shp_write); // // write END to last legal buffer slot too, in case we overflowed // ASSERT((MAX_TOKEN_CNT*5)<=RAMBUF_M2LIST_LEN); lfsr(FSR2,RAMBUF_M2LIST+((MAX_TOKEN_CNT-1)*5)); ASSERT(picRV(v_mo_angle) == ANGLE_END); rcall(w_shp_write); #if PIC_CSIM setSilent(oldSilent); picTrace(oldTrace); #endif // // done // picReturn(); ASSERT(0); } //=========================================================================== // w_mo2_savenext: //=========================================================================== static void w_mo2_savenext(void) { int picRegInOutParam(v_mo2_bits); #define PICDST_BITb_mo2_cnt1b PICDST_BIT0 #define PICDST_BITb_mo2_added PICDST_BIT1 #define PICDST_BITb_mo2_savenext PICDST_BIT2 int picRegInOutParam(v_mo2_next_mh); int picRegInOutParam(v_mo2_next_ml); int picRegInOutParam(v_mo2_next_mdh); int picRegInOutParam(v_mo2_next_mdl); btfss(v_mo2_bits,b_mo2_savenext); picReturn(); bcf(v_mo2_bits,b_mo2_savenext); movf(FSR2H,w); movwf(v_mo2_next_mh); movf(FSR2L,w); movwf(v_mo2_next_ml); movf(FSR0H,w); movwf(v_mo2_next_mdh); movf(FSR0L,w); movwf(v_mo2_next_mdl); picReturn(); ASSERT(0); } //=========================================================================== // w_mo2_fill_circle_gaps: //=========================================================================== static void w_mo2_fill_circle_gaps(void) { #if PIC_CSIM int dbg = DBG_FILLGAP_MINIMAL; int oldSilent = setSilent(OPT_silent); int oldTrace; #endif // // local variables // int picRegLocal(v_mo2_next_mh); int picRegLocal(v_mo2_next_ml); int picRegLocal(v_mo2_next_mdh); int picRegLocal(v_mo2_next_mdl); int picRegLocal(v_mo2_shape0); int picRegLocal(v_mo2_cnt0); int picRegLocal(v_mo2_cnt1); int picRegLocal(v_mo2_delta_oct); int picRegLocal(v_mo2_gap_oct); int picRegLocal(v_mo2_total_cnt); #if PIC_CSIM if (dbg) { oldSilent = setSilent(0); oldTrace = picTrace(1); } else { oldTrace = picTrace(0); } #endif picLabel(w_mo2_outer_loop); #if PIC_CSIM if (!OPT_silent) { printf("start fillCircleGaps_picsim\n"); } #endif clrf(v_mo2_bits); clrf(v_mo2_total_cnt); // // copy motions to M3LIST // lfsr(FSR2,RAMBUF_M2LIST); lfsr(FSR0,RAMBUF_M3LIST); picLabel(w_mo2_copy_loop); rcall(w_shp_read); rcall(w_shp_write); incf(v_mo2_total_cnt,f); #if PIC_CSIM if (dbg) { Motion2 m; int idx = (picRV(FSR2H)<<8) + picRV(FSR2L); idx -= RAMBUF_M3LIST; idx /= 5; memset(&m,0,sizeof(m)); m.shape = picRV(v_mo_shape); m.maxmag = picRV(v_mo_maxmag); m.imp = picRV(v_mo_impl) + (picRV(v_mo_imph) << 8); m.angle = picRV(v_mo_angle); printMotion2("COPY ",(&m)-idx+2,&m,6); } #endif btfss(v_mo_angle,b_AFLG_END); bra(w_mo2_copy_loop); // // look for gaps as we copy it back // lfsr(FSR2,RAMBUF_M3LIST+5); lfsr(FSR0,RAMBUF_M2LIST+5); picLabel(w_mo2_look_loop); rcall(w_shp_read); #if PIC_CSIM if (dbg) { Motion2 m; int idx = (picRV(FSR2H)<<8) + picRV(FSR2L); idx -= RAMBUF_M3LIST; idx /= 5; memset(&m,0,sizeof(m)); m.shape = picRV(v_mo_shape); m.maxmag = picRV(v_mo_maxmag); m.imp = picRV(v_mo_impl) + (picRV(v_mo_imph) << 8); m.angle = picRV(v_mo_angle); printMotion2("READ1 ",&m-idx+2,&m,2); breakpt(); } #endif picLabel(w_mo2_look_loop2); btfsc(v_mo_angle,b_AFLG_END); bra(w_mo2_look_done); movff(v_mo_shape,v_mo2_shape0); movlw(1-2-1); movwf(v_mo2_cnt0); movwf(v_mo2_cnt1); movf(v_mo_angle,w); rcall(w_a2oct); movwf(v_mo_oct); #if PIC_CSIM if (dbg) { printf("START: shape0=%02x oct=%d\n", picRV(v_mo2_shape0), picRV(v_mo_oct)); } #endif bsf(v_mo2_bits,b_mo2_savenext); picLabel(w_mo2_top_loop); incf(v_mo2_cnt0,f); rcall(w_shp_write); #if PIC_CSIM if (dbg) { Motion2 m; int idx = (picRV(FSR0H)<<8) + picRV(FSR0L); idx -= RAMBUF_M2LIST; idx /= 5; memset(&m,0,sizeof(m)); m.shape = picRV(v_mo_shape); m.maxmag = picRV(v_mo_maxmag); m.imp = picRV(v_mo_impl) + (picRV(v_mo_imph) << 8); m.angle = picRV(v_mo_angle); printMotion2("WRITE2",&m-idx+2,&m,4); breakpt(); } #endif rcall(w_mo2_savenext); movff(v_mo_oct,v_mo2_delta_oct); negf(v_mo2_delta_oct); rcall(w_shp_read); #if PIC_CSIM if (dbg) { Motion2 m; int idx = (picRV(FSR2H)<<8) + picRV(FSR2L); idx -= RAMBUF_M3LIST; idx /= 5; memset(&m,0,sizeof(m)); m.shape = picRV(v_mo_shape); m.maxmag = picRV(v_mo_maxmag); m.imp = picRV(v_mo_impl) + (picRV(v_mo_imph) << 8); m.angle = picRV(v_mo_angle); printMotion2("READ3 ",&m-idx+2,&m,2); breakpt(); } #endif btfsc(v_mo_angle,b_AFLG_END); bra(w_mo2_look_done); btfsc(v_mo_shape,b_MSHF_PREV_PAUSE); bra(w_mo2_look_loop2); // // calc delta oct // movf(v_mo_angle,w); rcall(w_a2oct); movwf(v_mo_oct); addwf(v_mo2_delta_oct,f); // // Do we know what direction this circle is going? // btfsc(v_mo2_shape0,b_MSHF_MONO); bra(w_mo2_top_check_dir); // // do not know direction - figure it out // movlw(MSHF_MONO); movwf(v_mo2_shape0); btfsc(v_mo2_delta_oct,2); bsf(v_mo2_shape0,b_MSHF_DIR); picLabel(w_mo2_top_check_dir); // // delta oct direction // btfsc(v_mo2_shape0,b_MSHF_DIR); negf(v_mo2_delta_oct); movlw(7); andwf(v_mo2_delta_oct,f); // // change of direction? // if MONO is set and dir bit is different then break // btfss(v_mo_shape,b_MSHF_MONO); bra(w_mo2_top_checked_dir); movf(v_mo_shape,w); xorwf(v_mo2_shape0,w); btfsc(WREG,b_MSHF_DIR); bra(w_mo2_look_next); picLabel(w_mo2_top_checked_dir); ASSERT(picRV(v_mo2_delta_oct)!=0); // // is delta_oct ==1? // dcfsnz(v_mo2_delta_oct,f); bra(w_mo2_top_loop); //picLabel(w_mo2_top_done); // // optimization - skip to next if only 1 found // movf(v_mo2_cnt0,f); bn(w_mo2_look_next); // // was delta_oct ==2? // decfsz(v_mo2_delta_oct,f); bra(w_mo2_look_next); // // gap_oct = dir ? current_oct+1 current_oct-1 // incf(v_mo_oct,w); btfss(v_mo2_shape0,b_MSHF_DIR); addlw(-2); andlw(7); movwf(v_mo2_gap_oct); bcf(v_mo2_bits,b_mo2_cnt1b); bsf(v_mo2_bits,b_mo2_savenext); rcall(w_mo2_savenext); #if PIC_CSIM if (dbg) { printf("gap? cnt0=%d cnt1=%d gap_oct=%d\n", picRV(v_mo2_cnt0)+2, picRV(v_mo2_cnt1)+2+1, picRV(v_mo2_gap_oct)); } #endif picLabel(w_mo2_bot_loop); incf(v_mo2_cnt1,f); rcall(w_shp_write); #if PIC_CSIM if (dbg) { Motion2 m; int idx = (picRV(FSR0H)<<8) + picRV(FSR0L); idx -= RAMBUF_M2LIST; idx /= 5; memset(&m,0,sizeof(m)); m.shape = picRV(v_mo_shape); m.maxmag = picRV(v_mo_maxmag); m.imp = picRV(v_mo_impl) + (picRV(v_mo_imph) << 8); m.angle = picRV(v_mo_angle); printMotion2("WRITE4",&m-idx+2,&m,4); breakpt(); } #endif movff(v_mo_oct,v_mo2_delta_oct); negf(v_mo2_delta_oct); rcall(w_shp_read); #if PIC_CSIM if (dbg) { Motion2 m; int idx = (picRV(FSR2H)<<8) + picRV(FSR2L); idx -= RAMBUF_M3LIST; idx /= 5; memset(&m,0,sizeof(m)); m.shape = picRV(v_mo_shape); m.maxmag = picRV(v_mo_maxmag); m.imp = picRV(v_mo_impl) + (picRV(v_mo_imph) << 8); m.angle = picRV(v_mo_angle); printMotion2("READ5 ",&m-idx+2,&m,2); breakpt(); } #endif btfsc(v_mo_angle,b_AFLG_END); bra(w_mo2_bot_done); btfsc(v_mo_shape,b_MSHF_PREV_PAUSE); bra(w_mo2_bot_done); // // change of direction? // if MONO is set and dir bit is different then break // btfss(v_mo_shape,b_MSHF_MONO); bra(w_mo2_bot_checked_dir); movf(v_mo_shape,w); xorwf(v_mo2_shape0,w); btfsc(WREG,b_MSHF_DIR); bra(w_mo2_bot_done); picLabel(w_mo2_bot_checked_dir); // // find delta oct // movf(v_mo_angle,w); rcall(w_a2oct); movwf(v_mo_oct); addwf(v_mo2_delta_oct,f); // // delta oct direction // btfsc(v_mo2_shape0,b_MSHF_DIR); negf(v_mo2_delta_oct); movlw(7); andwf(v_mo2_delta_oct,f); ASSERT(picRV(v_mo2_delta_oct)!=0); // // is delta_oct ==1? // dcfsnz(v_mo2_delta_oct,f); bra(w_mo2_bot_loop); // is 1 - loop // // is delta oct == 2? // decfsz(v_mo2_delta_oct,f); bra(w_mo2_bot_done); // not 2 - done // // delta_oct==2 - keep going if first time // btg(v_mo2_bits,b_mo2_cnt1b); btfsc(v_mo2_bits,b_mo2_cnt1b); bra(w_mo2_bot_loop); // first time - loop picLabel(w_mo2_bot_done); #if PIC_CSIM if (dbg) { printf("check: cnt0=%d cnt1=%d gap_oct=%d\n", picRV(v_mo2_cnt0)+2, picRV(v_mo2_cnt1)+2, picRV(v_mo2_gap_oct)); } #endif // // place pointers at gap // movlw(5); subwf(v_mo2_next_ml,w); movwf(FSR2L); movlw(0); subwfb(v_mo2_next_mh,w); movwf(FSR2H); movf(v_mo2_next_mdh,w); movwf(FSR0H); movf(v_mo2_next_mdl,w); movwf(FSR0L); // // check for valid Gap-fill condition // (cnt0>=2-2 && cnt1>=2-2 && (cnt0+cnt1 >= 6-2-2)) // movf(v_mo2_cnt0,w); bn(w_mo2_look_loop); movf(v_mo2_cnt1,w); bn(w_mo2_look_loop); addwf(v_mo2_cnt0,w); addlw(-(6-2-2)); bnc(w_mo2_look_loop); // no gap - loop // // make sure we do not already have 256 motion entries. // If we do then do not add a gap. // movf(v_mo2_total_cnt,f); bz(w_mo2_look_loop); // // Found a Gap!! // movf(v_mo2_shape0,w); andlw(MSHF_DIR); iorlw(MSHP_CIRCLE_GAP|MSHF_MONO); movwf(v_mo_shape); clrf(v_mo_maxmag); clrf(v_mo_impl); clrf(v_mo_imph); movf(v_mo2_gap_oct,w); rcall(w_oct2a); movwf(v_mo_angle); incf(v_mo2_total_cnt,f); // total # of motion entries rcall(w_shp_write); #if PIC_CSIM if (dbg) { Motion2 m; int idx = (picRV(FSR0H)<<8) + picRV(FSR0L); idx -= RAMBUF_M2LIST; idx /= 5; memset(&m,0,sizeof(m)); m.shape = picRV(v_mo_shape); m.maxmag = picRV(v_mo_maxmag); m.imp = picRV(v_mo_impl) + (picRV(v_mo_imph) << 8); m.angle = picRV(v_mo_angle); printMotion2("WRITEG",&m-idx+2,&m,4); breakpt(); } #endif bsf(v_mo2_bits,b_mo2_added); bra(w_mo2_look_loop); // // check starting at next motion // picLabel(w_mo2_look_next); movf(v_mo2_next_mh,w); movwf(FSR2H); movf(v_mo2_next_ml,w); movwf(FSR2L); movf(v_mo2_next_mdh,w); movwf(FSR0H); movf(v_mo2_next_mdl,w); movwf(FSR0L); bra(w_mo2_look_loop); // // finished with this iteration // picLabel(w_mo2_look_done); movlw(ANGLE_END); movwf(v_mo_angle); rcall(w_shp_write); #if PIC_CSIM if (dbg) { Motion2 m; int idx = (picRV(FSR0H)<<8) + picRV(FSR0L); idx -= RAMBUF_M2LIST; idx /= 5; memset(&m,0,sizeof(m)); m.shape = picRV(v_mo_shape); m.maxmag = picRV(v_mo_maxmag); m.imp = picRV(v_mo_impl) + (picRV(v_mo_imph) << 8); m.angle = picRV(v_mo_angle); printMotion2("WRITEE",&m-idx+2,&m,4); breakpt(); } #endif btfsc(v_mo2_bits,b_mo2_added); bra(w_mo2_outer_loop); #if PIC_CSIM setSilent(oldSilent); picTrace(oldTrace); #endif // // done // picReturn(); ASSERT(0); } //=========================================================================== // w_mo3_find_weak_circles: //=========================================================================== static void w_mo3_find_weak_circles(void) { #if PIC_CSIM int dbg = DBG_FINDWEAK; int oldSilent = setSilent(OPT_silent); int oldTrace; #endif // // local variables // int picRegLocal(v_mo3_dcnt); int picRegLocal(v_mo3_ccnt); int picRegLocal(v_mo3_msl); int picRegLocal(v_mo3_msh); #if PIC_CSIM if (dbg) { oldSilent = setSilent(0); oldTrace = picTrace(1); } else { oldTrace = picTrace(0); } #endif // // loop through all motions // lfsr(FSR2,RAMBUF_M2LIST+5); picLabel(w_mo3_loop1); clrf(v_mo3_dcnt); clrf(v_mo3_ccnt); clrf(v_tmp); movff(FSR2L,v_mo3_msl); movff(FSR2H,v_mo3_msh); #if PIC_CSIM if (dbg) { printf("m[%2d] Restart\n",(picFSR2()-RAMBUF_M2LIST)/5); } #endif // // loop through all motions in circle // picLabel(w_mo3_loop2); incf(v_tmp,f); #if PIC_CSIM if (dbg) { printf("m[%2d] cc=%-2d dc%-2d v_tmp=%d\n", (picFSR2()-0x500)/5, picRV(v_mo3_ccnt), picRV(v_mo3_dcnt), picRV(v_tmp)); } #endif rcall(w_shp_read); ASSERT(AFLG_END == (1<<6)); btfsc(v_mo_angle,6); bra(w_mo3_loop2_done); #if 1 #if PIC_CSIM // // check for pause is unneccesary - will never be a circle if // preceded by pause // { int shape = picRV(v_mo_shape); ASSERT((shape & MSHF_PLUS) == 0); if (shape & MSHF_PREV_PAUSE) { shape &= MSHF_SHP_MASK; ASSERT(shape != MSHP_DIRTY_CIRCLE); ASSERT(shape != MSHP_CLEAN_CIRCLE); ASSERT(shape != MSHP_CIRCLE_GAP); } } #endif #else // // stop if we hit a pause // ASSERT(MSHF_PREV_PAUSE == (1<<7)); btfsc(v_mo_shape,7); bra(w_mo3_loop2_done); #endif // // what shape is this? // movf(v_mo_shape,w); andlw(MSHF_SHP_MASK); #if ALLOW_GAPS_IN_STRONG_CIRCLES // // CIRCLE GAPS do not count, but keep going // addlw(LOW(-MSHP_CIRCLE_GAP)); bz(w_mo3_loop2); #endif // // clean circle? // incf(v_mo3_ccnt,f); addlw(LOW(MSHP_CIRCLE_GAP-MSHP_CLEAN_CIRCLE)); bz(w_mo3_loop2); // // dirty circle? // decf(v_mo3_ccnt,f); incf(v_mo3_dcnt,f); addlw(LOW(MSHP_CLEAN_CIRCLE-MSHP_DIRTY_CIRCLE)); bz(w_mo3_loop2); decf(v_mo3_dcnt,f); picLabel(w_mo3_loop2_done); #if PIC_CSIM if (dbg) { printf("m[%2d] cc=%-2d dc%-2d v_tmp=%d CHECK\n", (picFSR2()-RAMBUF_M2LIST)/5, picRV(v_mo3_ccnt), picRV(v_mo3_dcnt), picRV(v_tmp)); } #endif // // if only 1 then it must not be a circle // dcfsnz(v_tmp,f); bra(w_mo3_non_circle); // // should not be more than 128 // ASSERT(picRV(v_tmp) < 128); // // if (ccnt >= 5 || (ccnt>0 && ccnt+dcnt >= 6)) then STRONG CIRCLE // if (ccnt > 0) then WEAK CIRCLE // else not a circle at all // movf(v_mo3_ccnt,w); bz(w_mo3_non_circle); movff(v_mo3_msl,FSR2L); movff(v_mo3_msh,FSR2H); addlw(LOW(-5)); bnn(w_mo3_strong_circle); addwf(v_mo3_dcnt,w); addlw(LOW(5-6)); bnn(w_mo3_strong_circle); // // weak circle - make the whole thing be a dirty circle // picLabel(w_mo3_weak_circle); rcall(w_shp_read); movf(POSTDEC2,f); // back up to shape #if PIC_CSIM if (dbg) { printf("m[%2d] v_tmp=%d FIX: Make dirty %02x\n", (picFSR2()-RAMBUF_M2LIST-4)/5, picRV(v_tmp), picRV(INDF2)); } #endif ASSERT(MSHP_DIRTY_CIRCLE == 0); ASSERT(MSHP_CLEAN_CIRCLE == 2); movlw(2); subwf(INDF2,f); // turn clean circle into dirty movf(POSTINC2,w); andlw(MSHF_SHP_MASK); bz(w_mo3_weak_next); // // oops - it was not a clean circle. Put it back // movlw(2); movf(POSTDEC2,f); // back up to shape addwf(POSTINC2,f); // putit back the way it was #if PIC_CSIM if (dbg) { printf("m[%2d] v_tmp=%d UNFIX:\n", (picFSR2()-RAMBUF_M2LIST-4)/5, picRV(v_tmp)); } #endif picLabel(w_mo3_weak_next); decfsz(v_tmp,f); bra(w_mo3_weak_circle); bra(w_mo3_loop1); // // strong circle - set MFLG_CIRCLE // picLabel(w_mo3_strong_circle); rcall(w_shp_read); movf(POSTDEC2,f); // back up to shape #if PIC_CSIM if (dbg) { printf("m[%2d] v_tmp=%d FIX: MFLG_CIRCL %02x\n", (picFSR2()-RAMBUF_M2LIST-4)/5, picRV(v_tmp), picRV(INDF2)); } #endif ASSERT(MSHF_CIRCLE == (1<<5)); bsf(POSTINC2,5); // shape - set MFLG_CIRCLE decfsz(v_tmp,f); bra(w_mo3_strong_circle); bra(w_mo3_loop1); // // not a circle // picLabel(w_mo3_non_circle); // // loop until done // btfss(v_mo_angle,b_AFLG_END); bra(w_mo3_loop1); #if PIC_CSIM setSilent(oldSilent); picTrace(oldTrace); #endif // // done // picReturn(); ASSERT(0); } //=========================================================================== // w_mo4_find_best_pulses: //=========================================================================== static void w_mo4_find_best_pulses(void) { #if PIC_CSIM int dbg = DBG_FINDPULSE; int oldSilent = setSilent(OPT_silent); int oldTrace; #endif // // local variables // int picRegLocal(v_mo4_best_imp_mh); int picRegLocal(v_mo4_best_imp_ml); int picRegLocal(v_mo4_best_imph); int picRegLocal(v_mo4_best_impl); int picRegLocal(v_mo4_best_mag); int picRegLocal(v_mo4_best_mag_mh); int picRegLocal(v_mo4_best_mag_ml); int picRegLocal(v_mo4_next_mh); int picRegLocal(v_mo4_next_ml); int picRegLocal(v_mo4_prev_angle); int picRegLocal(v_mo4_prev_shape); int picRegLocal(v_mo4_sum_imph); int picRegLocal(v_mo4_sum_impl); #if PIC_CSIM if (dbg) { oldSilent = setSilent(0); oldTrace = picTrace(1); } else { oldTrace = picTrace(0); } if (!OPT_silent) { printf("start w_mo4_find_best_pulses\n"); } #endif lfsr(FSR2,RAMBUF_M2LIST+5); ASSERT(MSHP_CLEAN_CIRCLE != (0xff & MSHF_SHP_MASK)); setf(v_mo_shape); picLabel(w_mo4_outer_loop); movf(v_mo_shape,w); movwf(v_mo4_prev_shape); rcall(w_shp_read); #if PIC_CSIM if (dbg) { Motion2 m; int idx = (picRV(FSR2H)<<8) + picRV(FSR2L); idx -= RAMBUF_M2LIST; idx /= 5; memset(&m,0,sizeof(m)); m.shape = picRV(v_mo_shape); m.maxmag = picRV(v_mo_maxmag); m.imp = picRV(v_mo_impl) + (picRV(v_mo_imph) << 8); m.angle = picRV(v_mo_angle); printMotion2("READ1 ",&m-idx+2,&m,2); breakpt(); } #endif btfsc(v_mo_angle,b_AFLG_END); bra(w_mo4_done); // // keep going if PULSE or (DIRTY_CIRCLE && was not CLEAN_CIRCLE before) // movf(v_mo_shape,w); andlw(MSHF_SHP_MASK); ASSERT(MSHP_DIRTY_CIRCLE == 0); bnz(w_mo4_ocheck_pulse); // // its a DIRTY_CIRCLE - check if previous was a CLEAN_CIRCLE // movf(v_mo4_prev_shape,w); andlw(MSHF_SHP_MASK); addlw(-MSHP_CLEAN_CIRCLE); bz(w_mo4_outer_loop); bra(w_mo4_start_inner); picLabel(w_mo4_ocheck_pulse); addlw(-MSHP_PULSE); bnz(w_mo4_outer_loop); picLabel(w_mo4_start_inner); movf(v_mo_maxmag,w); movwf(v_mo4_best_mag); movf(v_mo_impl,w); movwf(v_mo4_best_impl); movwf(v_mo4_sum_impl); movf(v_mo_imph,w); movwf(v_mo4_best_imph); movwf(v_mo4_sum_imph); movf(FSR2L,w); movwf(v_mo4_best_imp_ml); movwf(v_mo4_best_mag_ml); movf(FSR2H,w); movwf(v_mo4_best_imp_mh); movwf(v_mo4_best_mag_mh); picLabel(w_mo4_inner_loop); movf(v_mo_angle,w); movwf(v_mo4_prev_angle); movf(FSR2L,w); movwf(v_mo4_next_ml); movf(FSR2H,w); movwf(v_mo4_next_mh); rcall(w_shp_read); #if PIC_CSIM if (dbg) { Motion2 m; int idx = (picRV(FSR2H)<<8) + picRV(FSR2L); idx -= RAMBUF_M2LIST; idx /= 5; memset(&m,0,sizeof(m)); m.shape = picRV(v_mo_shape); m.maxmag = picRV(v_mo_maxmag); m.imp = picRV(v_mo_impl) + (picRV(v_mo_imph) << 8); m.angle = picRV(v_mo_angle); printMotion2("READ2 ",&m-idx+2,&m,2); breakpt(); } #endif btfsc(v_mo_angle,b_AFLG_END); bra(w_mo4_inner_done); // // stop if pause detected // btfsc(v_mo_shape,b_MSHF_PREV_PAUSE); bra(w_mo4_inner_done); // // keep going if PULSE or (DIRTY_CIRCLE && not MSHF_CIRCLE) // movf(v_mo_shape,w); andlw(MSHF_SHP_MASK); ASSERT(MSHP_DIRTY_CIRCLE == 0); bnz(w_mo4_icheck_pulse); // // its a DIRTY_CIRCLE - check if MSHF_CIRCLE is set // btfsc(v_mo_shape,b_MSHF_CIRCLE); bra(w_mo4_inner_done); bra(w_mo4_check_angle); picLabel(w_mo4_icheck_pulse); addlw(-MSHP_PULSE); bnz(w_mo4_inner_done); picLabel(w_mo4_check_angle); // // check delta angle < 6 // movf(v_mo4_prev_angle,w); subwf(v_mo_angle,w); rcall(w_adiff); btfsc(WREG,7); negf(WREG); addlw(-6); bc(w_mo4_inner_done); // // check delta oct < 2 // movf(v_mo4_prev_angle,w); rcall(w_a2oct); movwf(v_mo4_prev_angle); movf(v_mo_angle,w); rcall(w_a2oct); subwf(v_mo4_prev_angle,w); rcall(w_odiff); btfsc(WREG,7); negf(WREG); ASSERT(picRV(WREG) != 0); decfsz(WREG,w); bra(w_mo4_inner_done); // // check for max magnitude // movf(v_mo_maxmag,w); cpfslt(v_mo4_best_mag); bra(w_mo4_check_imp); // // this is the best magnitude // movwf(v_mo4_best_mag); movf(FSR2L,w); movwf(v_mo4_best_mag_ml); movf(FSR2H,w); movwf(v_mo4_best_mag_mh); picLabel(w_mo4_check_imp); // // sum impulse // movf(v_mo_impl,w); addwf(v_mo4_sum_impl,f); movf(v_mo_imph,w); addwfc(v_mo4_sum_imph,f); btfsc(STATUS,C); setf(v_mo4_sum_imph); // // check for max impulse // movf(v_mo_impl,w); subwf(v_mo4_best_impl,w); movf(v_mo_imph,w); subwfb(v_mo4_best_imph,w); bc(w_mo4_inner_loop); // // this is the best impulse // movf(v_mo_impl,w); movwf(v_mo4_best_impl); movf(v_mo_imph,w); movwf(v_mo4_best_imph); movf(FSR2L,w); movwf(v_mo4_best_imp_ml); movf(FSR2H,w); movwf(v_mo4_best_imp_mh); bra(w_mo4_inner_loop); // // decide whether to set PLUS // picLabel(w_mo4_inner_done); // // sum of impulse >= 0x500 // movf(v_mo4_sum_imph,w); addlw(-5); bnc(w_mo4_next); // // best_impulse and best_magnitude should be the same // movf(v_mo4_best_imp_ml,w); movwf(FSR0L); subwf(v_mo4_best_mag_ml,w); bnz(w_mo4_next); movf(v_mo4_best_imp_mh,w); movwf(FSR0H); subwf(v_mo4_best_mag_mh,w); bnz(w_mo4_next); // // All conditions met - make it a PLUS // movf(POSTDEC0,w); // back up to preceding shape bsf(INDF0,b_MSHF_PLUS); picLabel(w_mo4_next); movf(v_mo4_next_ml,w); movwf(FSR2L); movf(v_mo4_next_mh,w); movwf(FSR2H); bra(w_mo4_outer_loop); picLabel(w_mo4_done); #if PIC_CSIM setSilent(oldSilent); picTrace(oldTrace); #endif // // done // picReturn(); ASSERT(0); } //=========================================================================== // w_mo5_marklast: //=========================================================================== static void w_mo5_marklast(void) { #if PIC_CSIM int dbg = DBG_MARKLAST; int oldSilent = setSilent(OPT_silent); int oldTrace; if (dbg) { oldSilent = setSilent(0); oldTrace = picTrace(1); } else { oldTrace = picTrace(0); } #endif // // find end of motions // lfsr(FSR2,RAMBUF_M2LIST+5); picLabel(w_mo5_loop1); rcall(w_shp_read); btfss(v_mo_angle,6); bra(w_mo5_loop1); // // mark last 4 entries as non-plus (includes the END entry, so we are // really only marking 3) // movlw(4); movf(POSTDEC2,f); // point to shape of last entry picLabel(w_mo5_loop2); ASSERT(MSHF_PLUS==1); bcf(POSTDEC2,0); // turn off MSHF_PLUS bit in shape movf(POSTDEC2,f); // maxmag movf(POSTDEC2,f); // imph movf(POSTDEC2,f); // impl ASSERT(AFLG_START == (1<<5)); btfsc(POSTDEC2,5); // return if we hit start bra(w_mo5_done); decfsz(WREG,w); // loop until we have done 3 bra(w_mo5_loop2); picLabel(w_mo5_done); #if PIC_CSIM setSilent(oldSilent); picTrace(oldTrace); #endif picReturn(); ASSERT(0); } //=========================================================================== // w_mo6_tokenize: //=========================================================================== static void w_mo6_tokenize(void) { #if PIC_CSIM int dbg = DBG_TOKENIZE; int oldSilent = setSilent(OPT_silent); int oldTrace; if (dbg) { oldSilent = setSilent(0); oldTrace = picTrace(1); } else { oldTrace = picTrace(0); } #endif // // read each motion and convert to token // FSR2 points to motions // FSR2 points to tokens // lfsr(FSR2,RAMBUF_M2LIST+5); lfsr(FSR0,RAMBUF_MTOKENS); picLabel(w_mo6_loop); rcall(w_shp_read); #if 1 // // for sanity, clear dir bit for pulses - NOT NECESSARY!?!?!? // ASSERT(MSHP_PULSE == 4); ASSERT(MSHP_PULSE_PLUS == 5); ASSERT(MSHF_PLUS == 1); ASSERT(MSHF_SHP_MASK == 7); movf(v_mo_shape,w); xorlw(0x04); andlw(0x06); btfsc(STATUS,Z); // pulse? bcf(v_mo_shape,3); // yes - clear DIR bit #endif rrncf(v_mo_angle,w); andlw(0x0f); movwf(INDF0); swapf(v_mo_shape,w); andlw(0xf0); iorwf(POSTINC0,f); btfss(v_mo_angle,6); bra(w_mo6_loop); movf(POSTDEC0,w); setf(INDF0); bcf(INDF0,0); #if PIC_CSIM setSilent(oldSilent); picTrace(oldTrace); #endif picReturn(); ASSERT(0); } //=========================================================================== // w_mo7_one_circle: add circle or ocircle token. //=========================================================================== static void w_mo7_one_circle(void) { // // add a circle token (optional or not) // oct is calculated from v_mo7_circle_oct // v_mo7_circle_oct is incremented by dir and stored into v_mo7_circle_oct // int picRegInOutParam(v_mo7_circle_oct); // // Note: PICDST_BITb_mo7_endspells is global (end of spells) // Note: PICDST_BITb_mo7_prev_dir must match PICDST_BITb_MWN_DIR // int picRegInOutParam(v_mo7_bits); #define PICDST_BITb_mo7_was_23circle PICDST_BIT0 #define PICDST_BITb_mo7_was_pulse PICDST_BIT1 #define PICDST_BITb_mo7_optional PICDST_BIT2 #define PICDST_BITb_mo7_endspells PICDST_BIT3 #define PICDST_BITb_mo7_prev_dir PICDST_BIT6 movf(v_mo7_circle_oct,w); btfsc(TABLAT,b_MWN_DIR); bra(w_mo7_one_circle_cw); ASSERT(SSHP_OCIRCLE>SSHP_CIRCLE); addlw(1); andlw(7); movwf(v_mo7_circle_oct); addlw(1); andlw(7); rlncf(WREG,w); iorlw((SSHP_CIRCLE&7)<<4); btfsc(v_mo7_bits,b_mo7_optional); addlw(((SSHP_OCIRCLE-SSHP_CIRCLE)&7)<<4); movwf(POSTINC0); ASSERT(picRV16(FSR0L,FSR0H)<RAMBUF_STOKENS+RAMBUF_STOKENS_LEN); #if PIC_CSIM && DBG_LOADSPELL { int val = picRV(WREG); printf("%sWRITE circle %02x = %s %s\n", colorStr(4), val, spellTokenStr(val), colorStr(-1)); } #endif picReturn(); picLabel(w_mo7_one_circle_cw); ASSERT(SSHP_OCIRCLE>SSHP_CIRCLE); addlw(-1); andlw(7); movwf(v_mo7_circle_oct); addlw(-1); andlw(7); rlncf(WREG,w); iorlw((SSHP_CIRCLE&7)<<4); btfsc(v_mo7_bits,b_mo7_optional); addlw(((SSHP_OCIRCLE-SSHP_CIRCLE)&7)<<4); bsf(WREG,b_TOK_DIR); movwf(POSTINC0); ASSERT(picRV16(FSR0L,FSR0H)<RAMBUF_STOKENS+RAMBUF_STOKENS_LEN); #if PIC_CSIM && DBG_LOADSPELL { int val = picRV(WREG); printf("%sWRITE circle %02x = %s %s\n", colorStr(4), val, spellTokenStr(val), colorStr(-1)); } #endif picReturn(); } //=========================================================================== // w_mo7_one_pulse: add pulse or opulse token. W=oct //=========================================================================== static void w_mo7_one_pulse(void) { // // add pulse in direction of oct // int picRegInOutParam(v_mo7_bits); andlw(7); rlncf(WREG,w); iorlw((SSHP_OPULSE&7)<<4); ASSERT(SSHP_PULSE > SSHP_OPULSE); btfss(v_mo7_bits,b_mo7_optional); addlw(((SSHP_PULSE-SSHP_OPULSE)&7)<<4); movwf(POSTINC0); ASSERT(picRV16(FSR0L,FSR0H)<RAMBUF_STOKENS+RAMBUF_STOKENS_LEN); #if PIC_CSIM && DBG_LOADSPELL { int val = picRV(WREG); printf("%sWRITE pulse %02x = %s %s\n", colorStr(4), val, spellTokenStr(val), colorStr(-1)); } #endif picReturn(); } #if PIC_CSIM static void spell_read_byte(void); #endif //=========================================================================== // w_mo7_load_spell: load the next spell's tokens //=========================================================================== // // Input values // TBLPTR - ptr to next spell // Output values // WREG - 0=end-of-spells, 0xff=got spell // TBLPTR - ptr to next spell // w_mo_spellinfol,h - ptr to spellinfo for spell // w_mo_spell_len - # of tokens in spell // RAMBUF_STOKENS - contains spell tokens // static void w_mo7_load_spell(void) { #if PIC_CSIM int dbg = DBG_LOADSPELL; int oldSilent = setSilent(OPT_silent); int dbg_cnt; int oldTrace; if (dbg) { oldSilent = setSilent(0); oldTrace = picTrace(1); } else { oldTrace = picTrace(0); } #define SPELL_READ_BYTE() \ spell_read_byte() #else #define SPELL_READ_BYTE() \ tblrd*+ #endif // // parameters & return values // int picRegReturn(v_mo_spellinfoh); int picRegReturn(v_mo_spellinfol); int picRegReturn(v_mo_maxscore); int picRegReturn(v_mo_holdoff); int picRegReturn(v_mo_id); // // locals // #if PIC_CSIM int picRegLocal(v_tmp); #endif int picRegReturn(v_mo7_bits); int picRegLocal(v_mo7_circle_oct); int picRegLocal(v_mo7_cnt0); int picRegLocal(v_mo7_cnt1); // // START // clrf(v_mo7_bits); SPELL_READ_BYTE(); // spellinfol movf(TABLAT,w); movwf(v_mo_spellinfol); SPELL_READ_BYTE(); // spellinfoh movf(TABLAT,w); movwf(v_mo_spellinfoh); // // check for end of spells (spellinfo = 0xffff) // ASSERT(PIC_MWN_END_SPELLS == 0xffff); andwf(v_mo_spellinfol,w); infsnz(WREG,w); bra(w_mo7_end_spells); SPELL_READ_BYTE(); // maxscore movf(TABLAT,w); movwf(v_mo_maxscore); SPELL_READ_BYTE(); // holdoff movf(TABLAT,w); movwf(v_mo_holdoff); SPELL_READ_BYTE(); // id movf(TABLAT,w); movwf(v_mo_id); SPELL_READ_BYTE(); // starting octant movf(TABLAT,w); movwf(v_mo_oct); lfsr(FSR0,RAMBUF_STOKENS); // // always start with optional pulse in initial direction // bsf(v_mo7_bits,b_mo7_optional); rcall(w_mo7_one_pulse); // // first one is special since it follows a pause // SPELL_READ_BYTE(); // first spell token btfss(TABLAT,7); bra(w_mo7_add_first_circle); ASSERT(PIC_MWN_MORE(picRV(WREG))); //picLabel(w_mo7_add_first_pulse); bcf(v_mo7_bits,b_mo7_optional); bra(w_mo7_pulse_common); picLabel(w_mo7_add_first_circle); decf(v_mo_oct,w); btfsc(TABLAT,b_MWN_DIR); addlw(2); movwf(v_mo7_circle_oct); // initial direction movlw(1); movwf(v_mo7_cnt0); // optional circles (leading) movf(TABLAT,w); andlw(MWN_CNT_MASK); addlw(2+8-1-2); movwf(v_mo7_cnt1); // required circles bra(w_mo7_circle_common); ASSERT(picRV(TABLAT) & (1<<PICDST_BITb_MWN_MORE)); picLabel(w_mo7_outer_loop); #if !PIC_CSIM #if 0 movff(FSR0L,v_mo_save_fsr0l); movff(FSR0H,v_mo_save_fsr0h); movff(FSR1L,v_mo_save_fsr1l); movff(FSR1H,v_mo_save_fsr1h); movff(FSR2L,v_mo_save_fsr2l); movff(FSR2H,v_mo_save_fsr2h); COUTL 'J' COUTL ' ' movf(v_mo_save_fsr0h,w); call(kk_outbyte); movf(v_mo_save_fsr0l,w); call(kk_outbyte); COUTL ' ' movff(v_mo_save_fsr0l,FSR0L); movff(v_mo_save_fsr0h,FSR0H); movf(POSTDEC0,w); movf(POSTINC0,w); call(kk_outbyte); call(kk_outcr); DB_FLUSH movff(v_mo_save_fsr0l,FSR0L); movff(v_mo_save_fsr0h,FSR0H); movff(v_mo_save_fsr1l,FSR1L); movff(v_mo_save_fsr1h,FSR1H); movff(v_mo_save_fsr2l,FSR2L); movff(v_mo_save_fsr2h,FSR2H); #endif #endif bcf(v_mo7_bits,b_mo7_optional); SPELL_READ_BYTE(); // next spell token btfss(TABLAT,7); bra(w_mo7_circle); // // ADDING PULSE // //picLabel(w_mo7_pulse); btfss(v_mo7_bits,b_mo7_was_pulse); bra(w_mo7_pulse_was_circle); // // was pulse or opulse // // opposite oct as prev? (same as current oct) // movf(TABLAT,w); subwf(v_mo_oct,w); // v_mo_oct is curent oct andlw(7); bz(w_mo7_pulse_common); // // same oct as prev? (opposite of current oct) // andlw(3); bnz(w_mo7_pulse_angle); bsf(v_mo7_bits,b_mo7_optional); movf(TABLAT,w); rcall(w_mo7_one_pulse); bra(w_mo7_pulse_common); // // some other angle - add circle segments to connect pulses // picLabel(w_mo7_pulse_angle); bsf(v_mo7_bits,b_mo7_optional); movf(v_mo_oct,w); movwf(v_mo7_circle_oct); subwf(TABLAT,w); rcall(w_odiff); bcf(TABLAT,b_MWN_DIR); btfsc(WREG,7); bsf(TABLAT,b_MWN_DIR); #if PIC_CSIM dbg_cnt = 0; #endif picLabel(w_mo7_pulse_angle_loop); #if PIC_CSIM dbg_cnt++; #endif rcall(w_mo7_one_circle); movf(v_mo7_circle_oct,w); subwf(TABLAT,w); andlw(7); bnz(w_mo7_pulse_angle_loop); ASSERT(dbg_cnt>0 && dbg_cnt<4); bcf(v_mo7_bits,b_mo7_optional); bra(w_mo7_pulse_common); picLabel(w_mo7_pulse_was_circle); // // a pulse following a circle // movf(POSTDEC0,f); // back up 4 tokens movf(POSTDEC0,f); movf(POSTDEC0,f); movf(POSTDEC0,f); // // cnt = -1 // setf(v_mo7_cnt0); // // use prev dir for circles // bcf(TABLAT,b_MWN_DIR); btfsc(v_mo7_bits,b_mo7_prev_dir); bsf(TABLAT,b_MWN_DIR); // // v_tmp = -dir (dir ? 1 : -1) // setf(v_tmp); btfsc(v_mo7_bits,b_mo7_prev_dir); negf(v_tmp); // // oct = (last_oct - dir * 2) & 7; // movf(v_mo_oct,w); addwf(v_tmp,w); addwf(v_tmp,w); // // if preceded by more than 1 circle (ie 2 or 3) // btfss(v_mo7_bits,b_mo7_was_23circle); bra(w_mo7_pulse_circles1); movf(POSTDEC0,f); // back up a 5th token decf(v_mo7_cnt0,f); // cnt = -2 addwf(v_tmp,w); // oct = oct - dir picLabel(w_mo7_pulse_circles1); movwf(v_mo7_circle_oct); bra(w_mo7_pulse_circles1_loop_start); // // add circles (preceding the pulse) // picLabel(w_mo7_pulse_circles1_loop); rcall(w_mo7_one_circle); incf(v_mo7_cnt0,f); picLabel(w_mo7_pulse_circles1_loop_start); // // while((octc+prev_dir)&7) != oct) // movf(v_tmp,w); subwf(v_mo7_circle_oct,w); subwf(TABLAT,w); andlw(7); bnz(w_mo7_pulse_circles1_loop); // // if less than a full circle then add a whole optional circle // bsf(v_mo7_bits,b_mo7_optional); btfss(v_mo7_cnt0,7); bra(w_mo7_pulse_circles3); movlw(8); movwf(v_mo7_cnt0); picLabel(w_mo7_pulse_circles2_loop); rcall( |