add_mscur_to_pos_ov MOVFF mscur,regd0 MOVF maxpos3,W,AC CPFSEQ curpos3,AC BRA addmscur MOVF maxpos2,W,AC CPFSEQ curpos2,AC BRA addmscur MOVF maxpos1,W,AC CPFSEQ curpos1,AC BRA addmscur MOVF maxpos1,W,AC CPFSEQ curpos1,AC BRA addmscurtst0 ;is equal addmscurtsteq CLRF curpos3,AC CLRF curpos2,AC CLRF curpos1,AC CLRF curpos0,AC BRA addmscur addmscurtst0 ;here we have to test, how wide cupos0 + mscur overflows maxpos ;first subtracting curpos0 from maxpos0, showing the diff MOVF curpos0,W,AC SUBWF maxpos0,W,AC ;sub curpos0 from maxpos0, store diff to wreg ;then first testing if mscur (regd0) is < diff CPFSLT regd0,AC BRA addmscurtsteqgr BRA addmscur ;we can do normal addition, ;maxpos will not be reached addmscurtsteqgr ;mscur is greater, or equal diff ;so we have to test if mscur (regd0) is equal to diff CPFSGT regd0,AC BRA addmscurtsteq ;is equal, so we set all to 0 ;and then add mscur to 0 ;here we produce more overflow, means mscur is greater then diff ;so we subs diff from mscur ;w contains diff SUBWF regd0,1,AC ;sub diff(w) from mscur(regd0) store to regd0 BRA addmscurtsteq ;regd0 contains rest, so ;going to set all 0, and set add with rest. addmscur MOVF regd0,W,AC ADDWF curpos0,1,AC CLRF WREG,AC ADDWFC curpos1,1,AC ADDWFC curpos2,1,AC ADDWFC curpos3,1,AC RETURN ;max 38 cycles sub_mscur_from_pos_ov MOVFF mscur,regd0 MOVLW 0x00 CPFSEQ curpos3,AC BRA submscur CPFSEQ curpos2,AC BRA submscur CPFSEQ curpos1,AC BRA submscur CPFSEQ curpos1,AC BRA submscurtst0 ;is equal submscurtsteq MOVFF curpos3,maxpos3 MOVFF curpos2,maxpos2 MOVFF curpos1,maxpos1 MOVFF curpos0,maxpos0 INCF regd0,1,AC ;we have to to this because underflow ;needs maxpos - 1 BRA submscur submscurtst0 ;here we have to test, how wide cupos0 - mscur underflows 0 ;first testing, if curpos0 is greater then mscur MOVF regd0,W,AC CPFSGT curpos0,AC BRA submscurtsteqsm BRA submscur ;no problem curpos is greater then mscur ;we can do a normal sub submscurtsteqsm ;curpos0 is smaller, or equal to mscur CPFSLT curpos0,AC BRA submscurtsteq ;is equal, so we set all to maxpos ;and then sub mscur normal ;here we produce more underflow, curpos is < mscur ;means curpos - mscur gives a value < 0 ;so we have to sub curpos from mscur, setting all to maxpos ;and from this to sub the rest. MOVF curpos0,W,AC SUBWF regd0,1,AC ;sub curpos0(w) from mscur(regd0), ;and store rest to regd0 BRA submscurtsteq submscur MOVF regd0,W,AC SUBWF curpos0,1,AC CLRF WREG,AC SUBWFB curpos1,1,AC SUBWFB curpos2,1,AC SUBWFB curpos3,1,AC RETURN ;max 38 cycles ;proofed 1 ;this sets ;work_flags.POS_FARAWAY will be set if current, and target ;differs over 1024 / 1/32 microsteps ( = 32 fullsteps ) ;If only differs lower, or equal 512 POS_FARAWAY is cleared. test_faraway BSF work_flags,POS_FARAWAY,AC MOVF target3,W,AC CPFSEQ curpos3,AC BRA tstfwend ;if highest bytes not equal, we are very far away MOVF target2,W,AC CPFSEQ curpos2,AC BRA tstfwend ;dito for bits 16-24 MOVF curpos1,W,AC ANDLW 0xfc MOVWF regd0,AC MOVF target1,W,AC ANDLW 0xfc CPFSEQ regd0,AC BRA tstfwend ;dito for bits 9-16 ;for all other, we clear it. BCF work_flags,POS_FARAWAY,AC tstfwend RETURN ;19 cycles max with call, return adapt_now ;we do nothing is msmax not greater then msmin MOVF msmin,W,AC CPFSGT msmax,AC RETURN ;we adpapt only if current position is ;one of the 4 valid fullstep-positions MOVLW D'0000' CPFSEQ stepctr,AC MOVLW D'0064' CPFSEQ stepctr,AC MOVLW D'0128' CPFSEQ stepctr,AC MOVLW D'0192' CPFSEQ stepctr,AC RETURN ;equal CALL test_faraway ;19 cycles BTFSS work_flags,POS_FARAWAY,AC BRA adaptnear adaptfar MOVF msmax,W,AC CPFSLT mscur,AC RETURN ;mscur is greater, or equal msmax, nothing to do RLCF mscur,1,AC ;thats all RETURN adaptnear MOVF msmin,W,AC CPFSGT mscur,AC RETURN ;mscur is smaller, or equal msmin, nothing to do BTFSC STATUS,C,AC RRCF mscur,1,AC ;thats all RETURN ;max 45 cycles with call, return, and test_faraway ;stepctr ;FS1 FS2 FS3 FS4 ;HS1 HS2 HS3 HS4 HS5 HS6 HS7 HS8 Over-/underflow ;--------------------------------------------------------------- ; 0 32 64 96 128 160 192 224 (256) step_now MOVF mscur,W,AC BTFSS work_flags,RUN_LEFTDOWN BRA step_right step_left SUBWF stepctr,1,AC BRA step_out step_right ADDWF stepctr,1,AC step_out MOVLW UPPER pwmtable MOVWF TBLPTRU MOVLW HIGH pwmtable MOVWF TBLPTRH MOVLW LOW pwmtable MOVWF TBLPTRL MOVF stepctr,W,AC MULLW D'0006' MOVFF PRODH,TBLPTRH MOVFF PRODL,TBLPTRL TBLRD*+ MOVFF TABLAT,CCPR1L TBLRD*+ MOVFF TABLAT,CCP1CON TBLRD*+ MOVFF TABLAT,CCPR2L TBLRD*+ MOVFF TABLAT,CCP2CON TBLRD*+ MOVFF TABLAT,PORTA RETURN ;max 36 cycles ;encodervalue has changed, means enc_tmp is unequal enc_tmpold ;new value is in enc_tmp, old value in enc_tmpold ;We have to test, which direction is used, and set work_flags.ENC_LEFTDOWN,AC ;rule for MENCB(bit1),MENCA(bit 0) 00 10 11 01 00 10 11 01 00 10 ...... ;new when left old new when right ; 01 00 10 ; 11 01 00 ; 00 10 11 ; 10 11 01 ; enc_now BCF work_flags,ENC_LEFTDOWN,AC MOVLW 0x03 CPFSLT enc_tmpold,AC BRA encnow11 MOVLW 0x02 CPFSLT enc_tmpold,AC BRA encnow10 MOVLW 0x01 CPFSLT enc_tmpold,AC BRA encnow01 encnow00 MOVLW 0x02 CPFSEQ enc_tmp,AC BSF work_flags,ENC_LEFTDOWN,AC RETURN encnow01 MOVLW 0x00 CPFSEQ enc_tmp,AC BSF work_flags,ENC_LEFTDOWN,AC RETURN encnow10 MOVLW 0x03 CPFSEQ enc_tmp,AC BSF work_flags,ENC_LEFTDOWN,AC RETURN encnow11 MOVLW 0x01 CPFSEQ enc_tmp,AC BSF work_flags,ENC_LEFTDOWN,AC RETURN ;max 16 cycles ;make single-step work depending on config ;this is called from program-loop, ;or directly form interrupt ( before doing any others ) ;( if INT_AS_FRQ is set ) STEP_WORK BTFSS run_flags,STEP_ON,AC BRA ENC_WORK ;if it is not switched on, we do only encoderwork DECF steplpctr,1,AC MOVLW 0xff CPFSEQ steplpctr,AC BRA ENC_WORK ;skip if == 0xff MOVFF steplp,steplpctr ;First we have to test if a step is necessary, ;means if current == target, then go to ENC_WORK CALL test_ifposequaltrg ;20 cycles BTFSS work_flags,POS_EQUAL,AC BRA dostepwork ;here current == target BCF run_flags,STEP_ON,AC BCF PORTC,TX,AC BRA ENC_WORK dostepwork BTFSS drv_flags,NO_CIRCLE,AC BRA dostepworkcircle BCF work_flags,RUN_LEFTDOWN,AC CALL test_pos ;max 38 cycles BTFSS work_flags,POS_SMALLER,AC BSF work_flags,RUN_LEFTDOWN,AC BRA adaptstepsize dostepworkcircle CALL scan_direction ;max 107 cycles adaptstepsize BSF PORTC,TX,AC CALL adapt_now ;max 45 cycles ;here we do our really stepping, which effects the motors ;using mscur, and run_flags.RUN_LEFTDOWN CALL step_now ;36 cycles max calcnewpos ;If RUN_LEFTDOWN, we have to decrease curpos0,1,2,3 with mscur ;if not, we have to increase it with mscur ;Doing decreasing/increasing with over/underflowtest to maxpos0,1,2,3 BTFSS work_flags,RUN_LEFTDOWN,AC CALL add_mscur_to_pos_ov ;38 cycles BTFSC work_flags,RUN_LEFTDOWN,AC CALL sub_mscur_from_pos_ov ;38 cycles endnewpos CALL test_ifposequaltrg ;20 cycles BTFSS work_flags,POS_EQUAL,AC BRA ENC_WORK ;here current == target BCF run_flags,STEP_ON,AC BCF PORTC,TX,AC ENC_WORK ;after each step ( if made, or not ) we have to test encoders. ;first reading encoder inputsignals CLRF enc_tmp BTFSC PORTB,MENCA,AC BSF enc_tmp,MENCA,AC BTFSC PORTB,MENCB,AC BSF enc_tmp,MENCB,AC ;then compare it with old values MOVF enc_tmp,W,AC CPFSEQ enc_tmpold,AC BRA encvaluechanged BRA endencpos encvaluechanged ;encodervalue has changed ;new value is in enc_tmp, old value in enc_tmpold ;We have to test, which direction is used, and set work_flags.ENC_LEFTDOWN,AC CALL enc_now ;16 cycles ;at least, for next changing MOVFF enc_tmp,enc_tmpold calcnewencpos ;Here we have to increase/decrease encHigh,encLow ;Doing decreasing/increasing with over/underflowtest to encMaxHigh,encMaxLow BTFSS work_flags,ENC_LEFTDOWN,AC CALL inc_encval_ov BTFSC work_flags,ENC_LEFTDOWN,AC CALL dec_encval_ov ;25 cycles endencpos MOVFF curpos3,spiobuf0 MOVFF curpos2,spiobuf1 MOVFF curpos1,spiobuf2 MOVFF curpos0,spiobuf3 MOVFF encHigh,spiobuf4 MOVFF encLow,spiobuf5 RETURN ;max with all calls, and returns 404 cycles