Title 2 Axis Stepper Controller Controller ;; Copyright 2009, Steven D. Wormley ;; All Rights Reserved ;; Please see ;; http://www.panomc.com/license.txt ;; for full rights granted ;; ;; stepcontrol3_001.asm list p=16f628 ; list directive to define processor #include ; processor specific variable definitions __CONFIG _CP_OFF & _WDT_ON & _BODEN_ON & _PWRTE_ON & _EXTCLK_OSC & _MCLRE_ON & _LVP_OFF ;;; Macros HZ_SET EQU D'1000' TICK_SET EQU D'1000000' / HZ_SET #define WS_TABLE_MASK 3 NUM_MOTOR EQU D'2' ;; Ports ;; RA0 step m0 ;; ra1 dir m0 ;; ra2 power m0 ;; ra3 gpo 0 m0 ;; ra4 gpo 1 m0 ;; rb0 step m1 ;; rb1 serial ;; rb2 serial ;; rb3 dir m1 ;; rb4 power m1 ;; rb5 gpo 0 m1 ;; rb6 gpo 1 m1 M0_STEP_PORT EQU PORTA M0_STEP_PIN EQU 0x00 M1_STEP_PORT EQU PORTB M1_STEP_PIN EQU 0x00 M0_POWER_PORT EQU PORTA M0_POWER_PIN EQU 0x02 M1_POWER_PORT EQU PORTB M1_POWER_PIN EQU 0x04 M0_DIRECTION_PORT EQU PORTA M0_DIRECTION_PIN EQU 0x01 M1_DIRECTION_PORT EQU PORTB M1_DIRECTION_PIN EQU 0x03 M0_OUT_0_PORT EQU PORTA M0_OUT_0_PIN EQU 0x03 M0_OUT_1_PORT EQU PORTA M0_OUT_1_PIN EQU 0x06 M1_OUT_0_PORT EQU PORTB M1_OUT_0_PIN EQU 0x05 M1_OUT_1_PORT EQU PORTB M1_OUT_1_PIN EQU 0x06 M0_SPEED_MIN EQU D'10' M1_SPEED_MIN EQU D'10' M0_SPEED_MAX EQU D'200' M1_SPEED_MAX EQU D'200' M0_DELTA_DELAY EQU D'10' M1_DELTA_DELAY EQU D'10' M0_DELTA_T EQU D'1' M1_DELTA_T EQU D'1' M0_POWER_DELAY EQU D'10' M1_POWER_DELAY EQU D'10' M0_PIN_SET EQU 0 M1_PIN_SET EQU 0 M0_POWER_L EQU 0 M0_POWER_H EQU 1 M1_POWER_L EQU 0 M1_POWER_H EQU 1 RX_PHASE_CONFIG_ID EQU 0x10 ALL_STOP EQU 0xde ; tx_phase to answer all stop ;; Macros ;; top 8 bits of a 24 bit value #define top(v) ((v >> 16 ) && 0xFF ) load_static_16 macro src,dst banksel dst movlw low(src) movwf dst movlw high(src) movwf dst+1 endm load_static_24 macro src,dst banksel dst movlw low(src) movwf dst movlw high(src) movwf dst+1 movlw (src >> 16 ) && 0xFF movwf dst+2 endm compare_unsigned_16 macro a1,a2 local results16 movf a1+1,w subwf a2+1,w ; subtract Y-X ; Are they equal ? skpz goto results16 ; yes, they are equal -- compare lo movf a1,w subwf a2,w ; subtract Y-X results16 ; if a1=a2 then now Z=1. ; if a2a2 then C=0 ; if a1<=a2 then now C=1. ENDM compare_unsigned_24 macro a1,a2 local results24 movf a1+2,w subwf a2+2,w ; subtract a2-a1 (17-24) ; Are they equal ? skpz goto results24 ; yes, they are equal -- compare mid movf a1+1,w subwf a2+1,w ; subtract a2-a1 ; Are they equal ? skpz goto results24 ; yes, they are equal -- compare low movf a1+0,w subwf a2+0,w ; subtract a2-a1 results24 ; if X=Y then now Z=1. ; if YY then C=0 ; if X<=Y then now C=1. ENDM addleft_16 MACRO a1,a2 ; add a2 to a1 and keep result in a1 movf a2,w addwf a1,f movf a2+1,w skpnc incfsz a2+1,w addwf a1+1,f ENDM addleft_24 MACRO a1,a2 ; add a2 to a1 and keep result in a1 movf a2,w addwf a1,f movf a2+1,w skpnc incfsz a2+1,w addwf a1+1,f movf a2+2,w skpnc incfsz a2+2,w addwf a1+2,f ENDM copy_16 MACRO src,dst movf src,w movwf dst movf src+1,w movwf dst+1 ENDM copy_24 MACRO src,dst movf src,w movwf dst movf src+1,w movwf dst+1 movf src+2,w movwf dst+2 ENDM copy_16_24 MACRO src,dst movf src,w movwf dst movf src+1,w movwf dst+1 clrf dst+2 ENDM copy_8_24 MACRO src,dst movf src,w movwf dst clrf dst+1 clrf dst+2 ENDM ;; DST = a1-SRC sub_16 MACRO a1,SRC,DST copy_16 a1,DST subleft_16 DST,SRC ENDM subleft_16 MACRO DST,SRC MOVF (SRC),W ; Get low byte of subtrahend SUBWF (DST),F ; Subtract DST(low) - SRC(low) MOVF (SRC)+1,W ; Now get high byte of subtrahend BTFSS STATUS,C ; If there was a borrow, rather than INCF (SRC)+1,W ; decrement high byte of dst we inc src SUBWF (DST)+1,F ; Subtract the high byte and we're done ENDM ;; DST = a1-SRC sub_24 MACRO a1,SRC,DST copy_24 a1,DST subleft_24 DST,SRC ENDM subleft_24 MACRO DST,SRC MOVF (SRC),W ; Get low byte of subtrahend SUBWF (DST),F ; Subtract DST(low) - SRC(low) MOVF (SRC)+1,W ; Now get high byte of subtrahend BTFSS STATUS,C ; If there was a borrow, rather than INCF (SRC)+1,W ; decrement high byte of dst we inc src SUBWF (DST)+1,F ; Subtract the high byte and we're done MOVF (SRC)+2,W ; Now get highest byte of subtrahend BTFSS STATUS,C ; If there was a borrow, rather than INCF (SRC)+2,W ; decrement high byte of dst we inc src SUBWF (DST)+2,F ; Subtract the high byte and we're done ENDM inc_16 MACRO DST INCFSZ (DST),W ; Add one to low byte DECF (DST)+1,F ; No carry (negates next step) INCF (DST)+1,F ; Add one to high byte MOVWF (DST) ; Store updated low byte back. IORWF (DST)+1,W ; Set Z flag ENDM dec_16 MACRO DST DECF (DST),F ; Decrement low byte INCFSZ (DST),W ; Check for underflow INCF (DST)+1,F ; Update DECF (DST)+1,F ; Fixup MOVF (DST),W IORWF (DST)+1,W ; Set Z bit ENDM inc_24 MACRO DST local inc_24_done INCFSZ (DST),F ; Add one to low byte GOTO inc_24_done INCFSZ (DST)+1,F ; Add one to high byte GOTO inc_24_done INCF (DST)+2,F ; Add one to the top byte inc_24_done: ENDM dec_24 MACRO DST local dec_24nz local dec_24nz_done local dec_24_done DECFSZ (DST),F ; decrement low byte GOTO dec_24nz MOVFW (DST)+1 IORWF (DST)+2,W GOTO dec_24_done dec_24nz INCFSZ (DST),W GOTO dec_24nz_done DECFSZ (DST)+1,F INCFSZ (DST)+1,W GOTO dec_24nz_done DECF (DST)+2,F dec_24nz_done clrz dec_24_done: ENDM ck_24_z MACRO CK movf CK,w iorwf (CK)+1,w iorwf (CK)+2,w ENDM ck_16_z MACRO CK movf CK,w iorwf (CK)+1,w ENDM clr_16 MACRO CL clrf CL clrf CL+1 ENDM clr_24 MACRO CL clrf CL clrf CL+1 clrf CL+2 ENDM set_motor_bank MACRO M bcf STATUS,RP0 bcf STATUS,RP1 movf M,w btfss STATUS,Z bsf STATUS,RP0 endm ;; ;; -------------------- ;; USER RAM DEFINITIONS ;; -------------------- ;; f_out_0 equ 0 f_out_1 equ 1 f_stepoff equ 2 f_ismove equ 3 f_movedone equ 4 f_pwrlow equ 5 f_pwrhigh equ 6 f_pwrdec equ 7 f_g_conffail EQU 0 f_g_confload EQU 1 f_g_confsave EQU 2 f_g_confdefault EQU 3 f_g_varsetok EQU 4 FLAG_VARIABLE_MASK equ 0x9b FLAG_STATIC_MASK equ 0x63 FLAG_POWER_MASK equ 0x9f CBLOCK 0x20 isr_t: 3 m_speed_0: 3 m_spd_count_0: 3 m_count_0: 3 m_togo_0: 3 m_rampup_0: 3 m_delta_count_0: 2 m_phase_0 m_power_delay_count_0: 2 m_flags_0 ; ;; Semi-constants m_speed_min_0: 3 m_speed_max_0: 3 m_delta_delay_0: 2 m_delta_t_0: 2 m_power_delay_0: 2 tick: 2 stk: 16 fsr_temp pclath_temp stk_w_temp_temp ENDC CBLOCK 0x70 ;; These are the global defines ;; Pretend we're a 24 bit 6502 hz: 3 A: 3 X: 1 m w_temp status_temp stkptr stk_w_temp pin_temp ENDC CBLOCK 0xA0 isr_t_2: 3 m_speed_1: 3 m_spd_count_1: 3 m_count_1: 3 m_togo_1: 3 m_rampup_1: 3 m_delta_count_1: 2 m_phase_1 m_power_delay_count_1: 2 m_flags_1 ; ;; Semi-constants m_speed_min_1: 3 m_speed_max_1: 3 m_delta_delay_1: 2 m_delta_t_1: 2 m_power_delay_1: 2 ;; main stuff work_char work_dir work_motor rx_phase tx_phase global_flags eeprom_temp_intcon message_buf2 message_buf3 message_pclath main_t: 3 Y: 3 ENDC org 0x0000 goto MAIN org 0x0004 ISR movwf w_temp swapf STATUS,w movwf status_temp banksel fsr_temp movf FSR,w movwf fsr_temp movf PCLATH,w movwf pclath_temp clrf PCLATH movf stk_w_temp,w movwf stk_w_temp_temp movf m,w call PUSH_W banksel PIR1 btfss PIR1 , CCP1IF goto ISR_DONE CCP_MAIN ;; Main loop for ccp match(our timer) clrf m ; Start with motor 0 MOTOR_LOOP ;; We use the bank to determine the motor set_motor_bank m ;; because we use the banksel bits, all references are to motor 0 btfss m_flags_0,f_stepoff goto MOTOR_RUN call STEP_OFF movf m_phase_0,w ; only phase 0 btfss STATUS,Z ; other phases, normal goto MOTOR_RUN btfsc m_flags_0,f_pwrdec ; are we already decrementing power goto PWR_DEC ;; determine if it's time to reduce power ck_16_z m_power_delay_0 ; 0? skpnz ; Yes, no power change goto PWR_DONE ; is 0, done, no power change copy_16 m_power_delay_0,m_power_delay_count_0 bsf m_flags_0,f_pwrdec PWR_DEC dec_16 m_power_delay_count_0 btfss STATUS,Z ; if !0 goto MOTOR_LOOP_DONE ; do nothing call POWER_LOW ; 0; set power to low PWR_DONE bcf m_flags_0,f_stepoff bcf m_flags_0,f_pwrdec goto MOTOR_LOOP_DONE MOTOR_RUN bcf m_flags_0,f_stepoff bcf m_flags_0,f_pwrdec movf m_phase_0,w skpnz goto MOTOR_LOOP_DONE ;; we have a phase >0 ;; is it 1? sublw 1 skpz goto CK_PHASE_2 ;; phase 1 ;; copy m_count to X with /2 IN_PHASE_1 bcf STATUS,C rrf (m_count_0)+2,W movwf (isr_t)+2 rrf (m_count_0)+1,W movwf (isr_t)+1 rrf (m_count_0)+0,W movwf (isr_t)+0 compare_unsigned_24 isr_t,m_togo_0 btfss STATUS,C goto NEXT_PHASE_3 ;; Not over halfway compare_unsigned_16 m_delta_delay_0,m_delta_count_0 btfss STATUS,C goto INC_MDC clr_16 m_delta_count_0 copy_16_24 m_delta_t_0,isr_t addleft_24 m_speed_0,isr_t compare_unsigned_24 m_speed_max_0,m_speed_0 ;; if we exceed speed_max C=1 btfss STATUS,C goto INC_MDC incf m_phase_0,f copy_24 m_speed_max_0,m_speed_0 sub_24 m_count_0 , m_togo_0 , m_rampup_0 goto INC_MDC NEXT_PHASE_3 movlw 0x03 movwf m_phase_0 goto IN_PHASE_3 CK_PHASE_2 movf m_phase_0,w sublw 2 skpz goto CK_PHASE_3 IN_PHASE_2 compare_unsigned_24 m_rampup_0,m_togo_0 btfsc STATUS,C ; if C=0 then then it's time to change phases goto MOVE_MOTOR incf m_phase_0,f clrf m_delta_count_0+0 clrf m_delta_count_0+1 CK_PHASE_3 movf m_phase_0,w sublw 3 skpz goto MOVE_MOTOR IN_PHASE_3 compare_unsigned_16 m_delta_count_0,m_delta_delay_0 btfsc STATUS,C goto INC_MDC clrf m_delta_count_0+0 clrf m_delta_count_0+1 copy_16_24 m_delta_t_0,isr_t subleft_16 m_speed_0,isr_t compare_unsigned_24 m_speed_0,m_speed_min_0 ;; if we re at or below speed_max C=1 btfss STATUS,C goto INC_MDC copy_24 m_speed_min_0,m_speed_0 INC_MDC inc_16 m_delta_count_0 MOVE_MOTOR ck_24_z m_speed_0 ;; if equal Z=1 btfsc STATUS,Z goto CLEAR_MOTOR ;; increment the count by the current speed addleft_24 m_spd_count_0,m_speed_0 compare_unsigned_24 hz, m_spd_count_0 ;; if HZ <= m_spd_count then C=1 btfss STATUS,C goto MOTOR_LOOP_DONE ; m_spd_count still less than HZ subleft_24 m_spd_count_0,hz ; reduce m_spd_count call STEP_ON bsf m_flags_0,f_stepoff dec_24 m_togo_0 ;; if m_togo ==0 then we're done btfss STATUS,Z goto MOTOR_LOOP_DONE bsf m_flags_0,f_movedone clrf m_speed_0+0 clrf m_speed_0+1 clrf m_speed_0+2 CLEAR_MOTOR clrf m_phase_0 MOTOR_LOOP_DONE incf m,w movwf m sublw NUM_MOTOR btfss STATUS,Z goto MOTOR_LOOP ;; done moving ISR_DONE banksel PIR1 bcf PIR1 , CCP1IF call POP_W movwf m banksel fsr_temp movf pclath_temp,w movwf PCLATH movf fsr_temp,w movwf FSR movf stk_w_temp_temp,w movwf stk_w_temp swapf status_temp,w movwf STATUS swapf w_temp,f swapf w_temp,w retfie STEP_ON movf m,w skpz goto STEP_ON_1 banksel M0_STEP_PORT bsf M0_STEP_PORT,M0_STEP_PIN goto STEP_DONE STEP_ON_1 banksel M1_STEP_PORT bsf M1_STEP_PORT,M1_STEP_PIN goto STEP_DONE STEP_OFF movf m,w skpz goto STEP_OFF_1 banksel M0_STEP_PORT bcf M0_STEP_PORT,M0_STEP_PIN goto STEP_DONE STEP_OFF_1 banksel M1_STEP_PORT bcf M1_STEP_PORT,M1_STEP_PIN STEP_DONE set_motor_bank m return DIRECTION_HIGH movf m,w skpz goto DIRECTION_HIGH_1 banksel M0_DIRECTION_PORT bsf M0_DIRECTION_PORT,M0_DIRECTION_PIN return DIRECTION_HIGH_1 banksel M1_DIRECTION_PORT bsf M1_DIRECTION_PORT,M1_DIRECTION_PIN return DIRECTION_LOW movf m,w skpz goto DIRECTION_LOW_1 banksel M0_DIRECTION_PORT bcf M0_DIRECTION_PORT,M0_DIRECTION_PIN return DIRECTION_LOW_1 banksel M1_DIRECTION_PORT bcf M1_DIRECTION_PORT,M1_DIRECTION_PIN return POWER_LOW movf STATUS,w call PUSH_W set_motor_bank m btfss m_flags_0,f_pwrlow goto POWER_SET_LOW goto POWER_SET_HIGH POWER_HIGH movf STATUS,w call PUSH_W btfss m_flags_0,f_pwrhigh goto POWER_SET_LOW POWER_SET_HIGH banksel M0_POWER_PORT bsf M0_POWER_PORT,M0_POWER_PIN goto POWER_DONE POWER_SET_LOW banksel M0_POWER_PORT bcf M0_POWER_PORT,M0_POWER_PIN POWER_DONE call POP_W movwf STATUS return MOVE ;; call with m, A_l and A_h to # steps and X_l direction 0/1 set_motor_bank m clrf m_phase_0 clr_24 m_spd_count_0 clr_24 m_rampup_0 clr_16 m_delta_count_0 ck_24_z A skpnz goto MOVE_NO_STEPS ;; have steps to move copy_24 m_speed_min_0,m_speed_0 copy_24 A,m_count_0 copy_24 A,m_togo_0 call POWER_HIGH movf X,w skpnz goto MOVE_NZ call DIRECTION_HIGH goto MOVE_DIR_DONE MOVE_NZ call DIRECTION_LOW MOVE_DIR_DONE ; DIRECTION setting wrecks our bank set_motor_bank m bsf m_flags_0,f_ismove movlw 0x01 movwf m_phase_0 return MOVE_NO_STEPS clr_24 m_speed_0 clr_24 m_count_0 clr_24 m_togo_0 bsf m_flags_0,f_movedone return VARSET ;; simple, switch on direction(now, variable) ;; and copy banksel global_flags bsf global_flags, f_g_varsetok set_motor_bank m decf X,f decf X,f skpnz goto VARSET_MAX decf X,f skpnz goto VARSET_MIN decf X,f skpnz goto VARSET_DELTA_T decf X,f skpnz goto VARSET_DELTA_DELAY decf X,f skpnz goto VARSET_POWER_DELAY decf X,f skpnz goto VARSET_POWER_FLAGS decf X,f skpnz goto VARSET_GPO_FLAGS return VARSET_MAX copy_24 A,m_speed_max_0 return VARSET_MIN copy_24 A,m_speed_min_0 return VARSET_DELTA_T copy_16 A,m_delta_t_0 return VARSET_DELTA_DELAY copy_16 A,m_delta_delay_0 return VARSET_POWER_DELAY copy_16 A,m_power_delay_0 return VARSET_POWER_FLAGS movf m_flags_0,w andlw FLAG_POWER_MASK ; clear both bits movwf m_flags_0 btfsc A,0 bsf m_flags_0,f_pwrlow btfsc A,1 bsf m_flags_0,f_pwrhigh return VARSET_GPO_FLAGS movf A,w goto SET_GPO ; goto here, SET_GPO does our return. MAIN ;; setup ;; stack initalization movlw stk movwf stkptr clrwdt banksel PORTA clrf PORTA clrf PORTB movlw 0x07 banksel CMCON movwf CMCON banksel PIE1 clrf PIE1 banksel INTCON clrf INTCON movlw 0x80 banksel OPTION_REG movwf OPTION_REG banksel TRISA clrf TRISA movlw 0x06 movwf TRISB clrf m banksel m_phase_0 call STEP_OFF call POWER_LOW banksel m_phase_1 incf m,f call STEP_OFF call POWER_LOW ; USART banksel SPBRG movlw 0x19 movwf SPBRG banksel TXSTA movlw 0x24 ; TXEN(32),!SYNC,BRGH(4), movwf TXSTA banksel RCSTA movlw 0x90 ; SPEN(128 0x80) CREN(16 0x10) movwf RCSTA clrf m clr_24 A clrf X call MOVE incf m,f call MOVE ;; setup our variables call LOAD_DEFAULTS ;; ccp1 setup banksel CCP1CON clrf CCP1CON clrf T1CON clrf TMR1H clrf TMR1L ;; same bank for tick and CCPR1L movf tick+0,w movwf CCPR1L movf tick+1,w movwf CCPR1H movlw 0x0b ; Compare mode, special event 00001011 movwf CCP1CON ;; start timer 1 bsf T1CON,TMR1ON ;; enable interrupts banksel PIE1 bsf PIE1,CCP1IE bsf INTCON,PEIE bsf INTCON,GIE banksel rx_phase clrf rx_phase clrf tx_phase clrf work_dir clrf work_char MAIN_LOOP clrwdt banksel PIR1 btfss PIR1,RCIF goto TRANSMIT_PENDING ;; Handle received data movf RCREG,w banksel work_char movwf work_char ; movf work_char,w ; temp testing ; skpnz ; goto TRANSMIT_PENDING ;; check our one special case xorlw '!' skpz goto RX_PHASE_CHECK clrf m clr_24 A clrf X call MOVE incf m,f call MOVE movlw ALL_STOP banksel tx_phase movwf tx_phase goto RX_CLEAR RX_PHASE_CHECK banksel rx_phase movf rx_phase,w xorlw 0 ; redudant much? skpnz goto RX_PHASE_0 xorlw 0x01 ^ 0x00 skpnz goto RX_PHASE_1 xorlw 0x02 ^ 0x01 skpnz goto RX_PHASE_2 xorlw 0x03 ^ 0x02 skpnz goto RX_PHASE_3 xorlw RX_PHASE_CONFIG_ID ^ 0x03 skpnz goto RX_PHASE_CONFIG ;; Odd, shouldn't happen goto RX_CLEAR RX_PHASE_0 movf work_char,w ;; switchy xorlw 'x' skpnz goto NC_1 xorlw 'y' ^ 'x' skpnz goto NC_2 xorlw 'z' ^ 'y' skpnz goto NC_3 xorlw 'c' ^ 'z' skpnz goto NC_4 goto TRANSMIT_PENDING NC_1 clrf work_motor incf rx_phase,f goto TRANSMIT_PENDING NC_2 movlw 0x01 movwf work_motor incf rx_phase,f goto TRANSMIT_PENDING NC_3 movlw 0x02 movwf work_motor incf rx_phase,f goto TRANSMIT_PENDING NC_4 ;; configuration meta commands movlw RX_PHASE_CONFIG_ID movwf rx_phase goto TRANSMIT_PENDING RX_PHASE_1 ;; we look for either a direction or a variable flag here clrf work_dir incf rx_phase,f movf work_char,w ;; switchy xorlw '+' skpnz goto TRANSMIT_PENDING incf work_dir,f xorlw '-' ^ '+' skpnz goto TRANSMIT_PENDING incf work_dir,f xorlw 'x' ^ '-' ; maX skpnz goto TRANSMIT_PENDING incf work_dir,f xorlw 'n' ^ 'x' ; miN skpnz goto TRANSMIT_PENDING incf work_dir,f xorlw 't' ^ 'n' ; delta-T skpnz goto TRANSMIT_PENDING incf work_dir,f xorlw 'd' ^ 't' ; delta-Delay skpnz goto TRANSMIT_PENDING incf work_dir,f xorlw 'p' ^ 'd' ; Power-delay skpnz goto TRANSMIT_PENDING incf work_dir,f xorlw 'q' ^ 'p' ; Power flags l=bit 0; h=bit 1 (q has no reason) skpnz goto TRANSMIT_PENDING incf work_dir,f xorlw 'g' ^ 'q' ; General IO bits 0+1 skpnz goto TRANSMIT_PENDING goto RX_CLEAR RX_PHASE_2 clr_24 A incf rx_phase,f RX_PHASE_3 ;; math is hard(tm) movf work_char,w ;; is it a delimiter 0x0d 0x0a or ',' xorlw 0x0d skpnz goto RX_DELIMITER xorlw 0x0a ^ 0x0d skpnz goto RX_DELIMITER xorlw ',' ^ 0x0a skpnz goto RX_DELIMITER ;; not a delimiter, is it valid movlw '0' subwf work_char,w movwf X sublw 0x09 skpc goto RX_CLEAR ;; carry set, number between 0 and 9 inclusive ;; multiply current A_l and A_h clrc rlf A+0,f ; x2 rlf A+1,f rlf A+2,f movf A+2,w movwf Y+2 movf A+1,w movwf Y+1 movf A+0,w movwf Y+0 clrc ; A x2(above) x4 rlf Y+0,f rlf Y+1,f rlf Y+2,f clrc rlf Y+0,f rlf Y+1,f rlf Y+2,f ;; A += Y addleft_24 A,Y ;; now, add our value ;; A += X copy_8_24 X,main_t addleft_24 A,main_t ;; yay, done goto TRANSMIT_PENDING RX_DELIMITER ;; We've received an end of string marker, thus we must ;; process the string using work_dir A_h and A_l movf work_motor,w movwf m movf work_dir,w skpnz goto RX_MOVE decf work_dir,w skpnz goto RX_MOVE movf work_dir,w movwf X ;; A_? has value, X_l has variable to set call VARSET goto RX_CLEAR RX_MOVE ;; A has number of steps already, just set direction movf work_dir,w movwf X call MOVE goto RX_CLEAR RX_PHASE_CONFIG ;; previous byte was a configuration command request, ;; this byte should tell us what to do movf work_char, w xorlw 's' skpnz goto RX_CONFIG_SAVE xorlw 'l' ^ 's' skpnz goto RX_CONFIG_LOAD xorlw 'd' ^ 'l' skpnz goto RX_CONFIG_DEFAULT xorlw 'r' ^ 'd' skpnz goto RX_DUMP_HEX xorlw 't' ^ 'r' skpnz goto CONFIG_TICK xorlw 'h' ^ 't' skpnz goto CONFIG_HZ goto RX_CLEAR RX_CONFIG_SAVE banksel global_flags bsf global_flags, f_g_confsave call SAVE_TO_EEPROM banksel global_flags skpz bsf global_flags,f_g_conffail goto RX_CLEAR RX_CONFIG_LOAD banksel global_flags bsf global_flags,f_g_confload call LOAD_FROM_EEPROM banksel global_flags skpz bsf global_flags,f_g_conffail goto RX_CLEAR RX_CONFIG_DEFAULT banksel global_flags bsf global_flags,f_g_confdefault call LOAD_DEFAULTS goto RX_CLEAR RX_DUMP_HEX call RAM_DUMP_HEX goto RX_CLEAR CONFIG_TICK CONFIG_HZ RX_CLEAR ;; receive failed or all done banksel rx_phase clrf rx_phase clrf work_dir clrf work_motor TRANSMIT_PENDING banksel PIR1 btfss PIR1,TXIE ; ok to transmit? goto MAIN_LOOP banksel work_char movf work_char,w skpnz goto TX_CONTINUE clrf work_char banksel TXREG movwf TXREG goto MAIN_LOOP TX_CONTINUE movf tx_phase,w movwf X skpnz goto TX_PHASE_0 decf X,f skpnz goto TX_PHASE_1 clrf tx_phase goto MAIN_LOOP TX_PHASE_0 ;; see if we're interested in outputting anything clrf m TX_M_LOOP set_motor_bank m btfsc m_flags_0,f_ismove goto TX_GO btfsc m_flags_0,f_movedone goto TX_OK incf m,f movf m,w xorlw NUM_MOTOR skpz goto TX_M_LOOP ;; nothing interesting ;; check for other outputs banksel global_flags btfsc global_flags, f_g_varsetok goto TX_VARSETOK btfsc global_flags, f_g_confsave goto TX_CONFSAVE btfsc global_flags, f_g_confload goto TX_CONFLOAD btfsc global_flags, f_g_confdefault goto TX_CONFDEFAULT goto MAIN_LOOP TX_VARSETOK bcf global_flags,f_g_varsetok movlw 0x4 goto TX_SET_MESSAGE TX_CONFSAVE bcf global_flags,f_g_confsave movlw 0x05 goto TX_CONFFAILCK TX_CONFLOAD bcf global_flags,f_g_confload movlw 0x07 goto TX_CONFFAILCK TX_CONFDEFAULT bcf global_flags,f_g_confdefault movlw 0x09 TX_CONFFAILCK btfsc global_flags,f_g_conffail addlw 0x01 bcf global_flags,f_g_conffail goto TX_SET_MESSAGE TX_GO ;; message number 0+m bcf m_flags_0,f_ismove bcf STATUS,C goto TX_SET_MESSAGE TX_OK ;; messgae number 1+m bcf m_flags_0,f_movedone bcf STATUS,C addlw 0x02 TX_SET_MESSAGE ;message number in w call SET_MESSAGE movlw 0x01 movwf tx_phase TX_PHASE_1 ; continue outputting a message call SET_MESSAGE_1 banksel tx_phase skpnz clrf tx_phase goto MAIN_LOOP SET_MESSAGE banksel message_buf3 andlw WS_TABLE_MASK movwf message_buf3 addwf message_buf3,w addlw LOW(WS_TABLE) movwf message_buf3 movlw HIGH(WS_TABLE) skpnc addlw 0x01 ; now [w,buffer3] points to the lo byte of the address of the string. movwf PCLATH ; now [PCLATH,buffer3] points to the lo byte of the address of the string. movf message_buf3,w ; get lo byte of the address of the string call SET_MESSAGE_2 movwf message_buf2 ; get high byte incf PCLATH,f incfsz message_buf3,w decf PCLATH,f call SET_MESSAGE_2 movwf message_pclath return SET_MESSAGE_1 movf message_pclath,w movwf PCLATH movf message_buf2,w call SET_MESSAGE_2 andlw 0xff skpnz return banksel TXREG movwf TXREG incf PCLATH,f banksel message_buf2 incfsz message_buf2,f decf PCLATH,f retlw 0x01 ; 1 not done yet SET_MESSAGE_2 movwf PCL WS_TABLE retlw LOW(string0) retlw HIGH(string0) retlw LOW(string1) retlw HIGH(string1) retlw LOW(string2) retlw HIGH(string2) retlw LOW(string3) retlw HIGH(string3) retlw LOW(string4) retlw HIGH(string4) retlw LOW(string5) retlw HIGH(string5) retlw LOW(string6) retlw HIGH(string6) retlw LOW(string7) retlw HIGH(string7) retlw LOW(string8) retlw HIGH(string8) retlw LOW(string9) retlw HIGH(string9) retlw LOW(string10) retlw HIGH(string10) retlw LOW(string11) retlw HIGH(string11) retlw LOW(string12) retlw HIGH(string12) retlw LOW(string13) retlw HIGH(string13) retlw LOW(string14) retlw HIGH(string14) retlw LOW(string15) retlw HIGH(string15) string0: dt "XGO",0x0d,0x0a,0 string1: dt "YGO",0x0d,0x0a,0 string2: dt "XOK",0x0d,0x0a,0 string3: dt "YOK",0x0d,0x0a,0 string4: dt "VOK",0x0d,0x0a,0 string5: dt "WOK",0x0d,0x0a,0 string6: dt "WNO",0x0d,0x0a,0 string7: dt "ROK",0x0d,0x0a,0 string8: dt "RNO",0x0d,0x0a,0 string9: dt "DOK",0x0d,0x0a,0 string10: dt "DNO",0x0d,0x0a,0 string11: dt 0 string12: dt 0 string13: dt 0 string14: dt 0 string15: dt 0 LOAD_FROM_EEPROM ;; this is reasonably easy banksel EEADR ;; first motor 1 clrf Y ;; check the checksum ;;; EEADR in same bank as Y movf Y,w movwf EEADR movf EEDATA,w xorlw 0x42 skpz retlw 0xff ;oops, invalid incf Y,f ;; load Hz TICKHZ_LOAD: movlw hz movwf X call DO_LOAD incf Y,f movlw hz+2 subwf X,w skpnc goto TICKHZ_LOAD M0_LOAD movlw m_flags_0 movwf X movf Y,f movwf EEADR bsf EECON1,RD banksel m_flags_0 movf m_flags_0,w andlw FLAG_VARIABLE_MASK banksel EEDATA iorwf EEDATA,w banksel m_flags_0 movwf m_flags_0 banksel EEDATA ;; loop for the rest of m0 M0_LOOP incf X,f movlw tick+1 ; yes, not m0, but in the m0 bank subwf X,w skpc ;; carry not set. all done goto M1_LOAD call DO_LOAD goto M0_LOOP M1_LOAD movlw m_flags_1 movwf X movf Y,f movwf EEADR bsf EECON1,RD banksel m_flags_1 movf m_flags_1,w andlw FLAG_VARIABLE_MASK banksel EEDATA iorwf EEDATA,w banksel m_flags_1 movwf m_flags_1 banksel EEDATA ;; loop for the rest of m0 M1_LOOP incf X,f movlw m_power_delay_1 +2 subwf X,w skpc ;; carry not set. all done call LOAD_EEPROM_DONE call DO_LOAD goto M1_LOOP LOAD_EEPROM_DONE clrf m call SET_GPO_PINS incf m,f call SET_GPO_PINS retlw 0x00 DO_LOAD movf X,w movwf FSR incf Y,f movf Y,w movwf EEADR bsf EECON1,RD movf EEDATA,w movwf INDF return SAVE_TO_EEPROM banksel INTCON movf INTCON,w clrf INTCON btfsc INTCON,GIE ; make sure GIE stays cleared goto $-2 banksel eeprom_temp_intcon movwf eeprom_temp_intcon banksel EECON1 bsf EECON1,WREN ;; first clear the signature byte clrf Y movlw 0x00 call STORE_EEPROM_W incf Y,w TICKHZ_SAVE: movlw hz movwf X call DO_SAVE incf Y,f movlw hz+2 subwf X,w skpnc goto TICKHZ_SAVE M0_SAVE movlw m_flags_0 movwf X banksel m_flags_0 movf m_flags_0,w andlw FLAG_STATIC_MASK ; remove the bits that change call STORE_EEPROM_W banksel EEDATA ;; loop for the rest of m0 M0_LOOP_SAVE incf X,f movlw tick+1 subwf X,w skpc ;; carry not set. all done goto M1_SAVE call DO_SAVE goto M0_LOOP_SAVE M1_SAVE movlw m_flags_1 movwf X banksel m_flags_1 movf m_flags_1,w andlw FLAG_STATIC_MASK ; remove the bits that change call STORE_EEPROM_W banksel EEDATA ;; loop for the rest of m0 M1_LOOP_SAVE incf X,f movlw m_power_delay_1+1 subwf X,w skpc ;; carry not set. all done goto SAVE_DONE call DO_SAVE goto M1_LOOP_SAVE SAVE_DONE ;; set the signature byte clrf Y movlw 0x42 call STORE_EEPROM_W banksel eeprom_temp_intcon movf eeprom_temp_intcon,w banksel INTCON movwf INTCON banksel EECON1 bcf EECON1,WREN return DO_SAVE movf X,w movwf FSR incf Y,f movf INDF,w STORE_EEPROM_W banksel EEDATA movwf EEDATA movf Y,w movwf EEADR movlw 0x55 movwf EECON2 movlw 0xAA movwf EECON2 bsf EECON1,WR ;; wait for write to complete btfsc EECON1,WR goto $-1 return LOAD_DEFAULTS load_static_16 TICK_SET,tick load_static_24 HZ_SET,hz load_static_24 M0_SPEED_MIN,m_speed_min_0 load_static_24 M1_SPEED_MIN,m_speed_min_1 load_static_24 M0_SPEED_MAX,m_speed_max_0 load_static_24 M1_SPEED_MAX,m_speed_max_1 load_static_16 M0_DELTA_DELAY,m_delta_delay_0 load_static_16 M1_DELTA_DELAY,m_delta_delay_1 load_static_16 M0_DELTA_T,m_delta_t_0 load_static_16 M1_DELTA_T,m_delta_t_1 load_static_16 M0_POWER_DELAY,m_power_delay_0 load_static_16 M1_POWER_DELAY,m_power_delay_1 banksel m_flags_0 clrf m_flags_0 banksel m_flags_1 clrf m_flags_1 clrf m movlw M0_PIN_SET call SET_GPO incf m,f movlw M1_PIN_SET call SET_GPO clrf A banksel m_flags_0 movlw M0_POWER_L iorwf A,w skpz bsf m_flags_0,f_pwrlow movlw M0_POWER_H iorwf A,w skpz bsf m_flags_0,f_pwrhigh banksel m_flags_1 movlw M1_POWER_L iorwf A,w skpz bsf m_flags_1,f_pwrlow movlw M1_POWER_H iorwf A,w skpz bsf m_flags_1,f_pwrhigh return SET_GPO ;; Set the m_flags_(m) and also set the pins movwf pin_temp set_motor_bank m movlw 0xfc ; 11111100 andwf m_flags_0,f movf pin_temp,w iorwf m_flags_0,f ;; set the actual pins SET_GPO_PINS movf m,f skpz goto SET_GPO_1 banksel m_flags_0 btfsc m_flags_0, f_out_0 goto SET_GPO_0_0_1 banksel M0_OUT_0_PORT bcf M0_OUT_0_PORT,M0_OUT_0_PIN goto SET_GPO_0_0_DONE SET_GPO_0_0_1 banksel M0_OUT_0_PORT bsf M0_OUT_0_PORT,M0_OUT_0_PIN SET_GPO_0_0_DONE banksel m_flags_0 btfsc m_flags_0, f_out_1 goto SET_GPO_0_1_1 banksel M0_OUT_1_PORT bcf M0_OUT_1_PORT,M0_OUT_1_PIN goto SET_GPO_0_1_DONE SET_GPO_0_1_1 banksel M0_OUT_1_PORT bsf M0_OUT_1_PORT,M0_OUT_1_PIN SET_GPO_0_1_DONE return SET_GPO_1 banksel m_flags_1 btfsc m_flags_1, f_out_1 goto SET_GPO_1_0_1 banksel M1_OUT_0_PORT bcf M1_OUT_0_PORT,M1_OUT_0_PIN goto SET_GPO_1_0_DONE SET_GPO_1_0_1 banksel M1_OUT_0_PORT bsf M1_OUT_0_PORT,M1_OUT_0_PIN SET_GPO_1_0_DONE banksel m_flags_1 btfsc m_flags_1, f_out_1 goto SET_GPO_1_1_1 banksel M1_OUT_1_PORT bcf M1_OUT_1_PORT,M1_OUT_1_PIN goto SET_GPO_1_1_DONE SET_GPO_1_1_1 banksel M1_OUT_1_PORT bsf M1_OUT_1_PORT,M1_OUT_1_PIN SET_GPO_1_1_DONE return RAM_DUMP_HEX ;; dump ram 0x00 to 0xf0 movlw 0x00 movwf X RAM_DUMP_LOOP clrwdt movf X,w andlw 0x0f skpz goto RAM_DUMP_NEXT movlw "\r" call W_SEND movlw "\n" call W_SEND movf X,w movwf A call HEX_BYTE_OUT movlw ":" call W_SEND RAM_DUMP_NEXT clrwdt movlw " " call W_SEND movf X,w movwf FSR movf INDF,w movwf A call HEX_BYTE_OUT clrwdt incf X,f movf X,w sublw 0xef skpnc goto RAM_DUMP_LOOP movlw "\r" call W_SEND movlw "\n" call W_SEND clrwdt return HEX_BYTE_OUT swapf A,W call HEX_NIBBLE_OUT ;print Lbyte hi nibble in hex movf A,W HEX_NIBBLE_OUT movwf (A)+1 andlw 0x0f ;mask hi bits sublw 0x09 ;9 - W if >9, Cflag = 0 movf (A+1),W ;get data andlw 0x0f ;mask hi bits skpc ;is input >9 addlw 0x07 ;if >9 add 7 for A-F addlw "0" ;make ASCII W_SEND banksel TXSTA ;set bank 1 to access TXSTA reg btfss TXSTA,TRMT ;test for Tx buffer empty goto $ - 1 ;wait till buffer empty banksel TXREG ;back to bank 0 movwf TXREG ;send it return PUSH_W movwf stk_w_temp incf stkptr,f movf stkptr,w movwf FSR movf stk_w_temp,w movwf INDF return POP_W movf stkptr,w movwf FSR movf INDF,w decf stkptr,f return END