ORG 0x004 ; interrupt vector location movwf WSave ; save off current W register contents movf STATUS,w ; move status register into W register movwf StatusSave ; save off contents of STATUS register ; isr code can go here or be located as a call subroutine elsewhere btfsc INTCON,T0IF ;Was it a Capture call TMR0OverFlow btfsc PIR1,TMR1IF ;Was it a TMR0 overflow call TMR1OverFlow movf StatusSave,w ; retrieve copy of STATUS register movwf STATUS ; restore pre-isr STATUS register contents swapf WSave,f swapf WSave,w ; restore pre-isr W register contents retfie ; return from interrupt TMR1OverFlow: ;Toggles the Heartbeat status LED every 498.0736mS bcf PIR1,TMR1IF banksel Bank2 incfsz TMR1PostScaler,f return movlw D'218' ;fudge factor to get close to 1 sec flashes movwf TMR1PostScaler banksel PORTB movfw PORTB xorlw HeartBeatMask ;Toggle the heartbeat output movwf PORTB return ;// clear int flag straight away to give max safe time TMR0OverFlow: bcf INTCON,T0IF step1: ; 1. updates motor encoder to see if motor has moved, records it position ; enc_new = (PORTA & 0b00000011); // get the 2 encoder bits banksel PORTA ;TMR0,PORTA-B,PIR1,TMR1L,TMR1H,T1CON,TMR2,T2CON,CCPR1L,CCPR1H,CCP1CON,RCSTA,TXREG,RCREG,CMCON movfw PORTA andlw B'00000011' movwf m_enc_new ;get the motor encoders new state ; if(enc_new != enc_last) movfw m_enc_last subwf m_enc_new,w btfsc STATUS,Z goto m_last_new_same ;no change ie not moving ; if(enc_new.F1 != enc_last.F0) mpos++; // record new motor position btfss m_enc_new,1 ;testing MOTOR_ENC_B for high goto m_enc_new_zero ;not high btfss m_enc_last,0 ;was high now test prev MOTOR_ENC_A for high goto m_diff ;not high therefore position has changed goto m_same ;high therefore same m_enc_new_zero: btfss m_enc_last,0 ;test prev MOTOR_ENC_A for high goto m_same ;low therefore same goto m_diff ;high therefore position has changed m_diff: ;mpos++ incf mpos,f ;motor moved FWD, record it goto m_update_last m_same: ;mpos-- decf mpos,f ;motor moved REV, record it goto m_update_last m_update_last:; enc_last = enc_new movfw m_enc_new ;save current state for future comparison movwf m_enc_last m_last_new_same: step2: ; 2. updates thumbwheel encoder to see if motor has moved, records it position ; enc_new = (PORTA & 0b00000011); // get the 2 encoder bits ; As above but for the Thumbwheel banksel PORTA ;TMR0,PORTA-B,PIR1,TMR1L,TMR1H,T1CON,TMR2,T2CON,CCPR1L,CCPR1H,CCP1CON,RCSTA,TXREG,RCREG,CMCON movfw PORTA andlw B'00001100' movwf t_enc_new ; if(enc_new != enc_last) movfw t_enc_last subwf t_enc_new,w btfsc STATUS,Z goto t_last_new_same ; if(enc_new.F3 != enc_last.F2) tpos++; // record new thumbwheel position btfss t_enc_new,3 goto t_enc_new_zero btfss t_enc_last,2 goto t_diff goto t_same t_enc_new_zero: btfss t_enc_last,2 goto t_same goto t_diff t_diff: ;tpos++ incf tpos,f goto t_update_last t_same: ;tpos-- decf tpos,f goto t_update_last t_update_last:; enc_last = enc_new movfw t_enc_new movwf t_enc_last t_last_new_same: goto step3 step3: ; 3. compares the two, set PWM% if motor lags behind Thumbwheel ; if(mpos < tpos) // if motor lagging behind ; { movfw tpos subwf mpos,w btfsc STATUS,Z goto tpos_equal_mpos btfsc STATUS,C goto tpos_less_mpos ;calc the lag goto tpos_greater_mpos ;calc the lag ; else // else if motor is fast, cut all power! ;***************************************************************************** tpos_equal_mpos: clrf CCPR1L ;Set PWM off M_OFF ;Make sure both are set to output (outputs low) clrf mlag ;there is no lag goto step5 ;************************************************************************** tpos_greater_mpos: ; mlag = (tpos - mpos); // find how far it's lagging behind movfw mpos subwf tpos,w movwf mlag ; if(mlag >= (100/MOTOR_PROP_GAIN)) CCPR1L = 100; // full power if is too far behind movlw (D'100'/MOTOR_PROP_GAIN) subwf mlag,w btfss STATUS,C goto mlag_less ;Something less than full power M_FWD movlw D'100' ;Full Power movwf CCPR1L ; goto step4 mlag_less: ; else CCPR1L = (mlag * MOTOR_PROP_GAIN); // else adjust PWM if slightly behind movlw MOTOR_PROP_GAIN movwf FSR ;FSR is a handy register that is in all banks clrw next_mlag_add: ;multiply by multiple additions addwf mlag,w decfsz FSR goto next_mlag_add M_FWD movwf CCPR1L ;Update PWM goto step4 ;**************************************************************** tpos_less_mpos: ; { ; mlag = (tpos - mpos); // find how far it's lagging behind movfw tpos subwf mpos,w movwf mlag ; if(mlag >= (100/MOTOR_PROP_GAIN)) CCPR1L = 100; // full power if is too far behind movlw (D'100'/MOTOR_PROP_GAIN) subwf mlag,w btfss STATUS,C goto REV_mlag_less M_REV movlw D'100' movwf CCPR1L ;check this goto step4 REV_mlag_less: ; else CCPR1L = (mlag * MOTOR_PROP_GAIN); // else adjust PWM if slightly behind movlw MOTOR_PROP_GAIN movwf FSR clrw REV_next_mlag_add: addwf mlag,w decfsz FSR goto REV_next_mlag_add ; sublw D'100' ;To invert as a low turns on the H-bridge M_REV movwf CCPR1L goto step4 step4: ; // 4. limit both counts, so they never roll over, but still retain all error ; if(tpos>250 || mpos>250) movfw tpos addlw D'5' btfsc STATUS,C ;Should really check carry goto upper_limit_counts movfw mpos addlw D'5' btfsc STATUS,C ;Should really check carry goto upper_limit_counts goto step5 ; { ; rpos -= 50; ; mpos -= 50; upper_limit_counts: movlw D'50' subwf tpos,f subwf mpos,f ;************************************* ; } step5: ; // 4. limit both counts, so they never roll under, but still retain all error ; if(tpos>250 || mpos>250){ movlw D'5' subwf tpos,w btfss STATUS,C ;Should really check carry goto lower_limit_counts movlw D'5' subwf mpos,w btfss STATUS,C ;Should really check carry goto lower_limit_counts goto end_tmr0 ;}else { ; rpos -= 50; ; mpos -= 50; lower_limit_counts: movlw D'50' addwf tpos,f addwf mpos,f ;************************************* ; } end_tmr0 return