LIST P= 16F84A INCLUDE "P16F84A.INC" RADIX DEC ORG 0X0000 CLRF STATUS GOTO PROGRAM_START ORG 0X0004 ; SAVE CONTEXT (PIC REGISTERS) MOVWF W_TEMP SWAPF STATUS, W CLRF STATUS MOVWF STATUS_TEMP MOVF PCLATH, W MOVWF PCLATH_TEMP MOVF FSR, W MOVWF FSR_TEMP CLRF PCLATH BCF STATUS, RP0 ; SAVE CONTEXT (COMPILER'S VARIABLES) MOVF FSR_TEMP, W MOVWF TEMP_FSR_TEMP MOVF AUX1_H, W MOVWF TEMP_AUX1_H MOVF AUX1_L, W MOVWF TEMP_AUX1_L MOVF AUX2_H, W MOVWF TEMP_AUX2_H MOVF AUX2_L, W MOVWF TEMP_AUX2_L MOVF AUX, W MOVWF TEMP_AUX MOVF S0, W MOVWF TEMP_S0 MOVF S1, W MOVWF TEMP_S1 MOVF S2, W MOVWF TEMP_S2 MOVF S3, W MOVWF TEMP_S3 ; CHECK WHICH INTERRUPT OCCURED BTFSC INTCON, INTF GOTO EXT_INTERRUPT_ROUTINE ; RESTORE CONTEXT (COMPILER'S VARIABLES) RESTORE_CONTEXT MOVF TEMP_FSR_TEMP, W MOVWF FSR_TEMP MOVF TEMP_AUX1_H, W MOVWF AUX1_H MOVF TEMP_AUX1_L, W MOVWF AUX1_L MOVF TEMP_AUX2_H, W MOVWF AUX2_H MOVF TEMP_AUX2_L, W MOVWF AUX2_L MOVF TEMP_AUX, W MOVWF AUX MOVF TEMP_S0, W MOVWF S0 MOVF TEMP_S1, W MOVWF S1 MOVF TEMP_S2, W MOVWF S2 MOVF TEMP_S3, W MOVWF S3 ; RESTORE CONTEXT (PIC REGISTERS) MOVF FSR_TEMP, W MOVWF FSR MOVF PCLATH_TEMP, W MOVWF PCLATH SWAPF STATUS_TEMP, W MOVWF STATUS SWAPF W_TEMP, F SWAPF W_TEMP, W RETFIE ; HEADER END W_TEMP EQU 0X4F STATUS_TEMP EQU 0XC PCLATH_TEMP EQU 0XD FSR_TEMP EQU 0XE AUX1_H EQU 0XF AUX1_L EQU 0X10 AUX2_H EQU 0X11 AUX2_L EQU 0X12 AUX EQU 0X13 S0 EQU 0X14 S1 EQU 0X15 S2 EQU 0X16 S3 EQU 0X17 TEMP_FSR_TEMP EQU 0X18 TEMP_AUX1_H EQU 0X19 TEMP_AUX1_L EQU 0X1A TEMP_AUX2_H EQU 0X1B TEMP_AUX2_L EQU 0X1C TEMP_AUX EQU 0X1D TEMP_S0 EQU 0X1E TEMP_S1 EQU 0X1F TEMP_S2 EQU 0X20 TEMP_S3 EQU 0X21 XIN EQU 0X22 YIN EQU 0X23 ZIN EQU 0X24 FSTART EQU 0X25 XCNT EQU 0X26 YCNT EQU 0X27 ZCNT EQU 0X28 INIT EQU 0X29 XCENT EQU 0X2A YCENT EQU 0X2B ZCENT EQU 0X2C AUXUP EQU 0X2D AUXDN EQU 0X2E YHI EQU 0X2F YLO EQU 0X30 XHI EQU 0X31 XLO EQU 0X32 YOFF EQU 0X33 XOFF EQU 0X34 GREU8 BCF STATUS, RP0 MOVF AUX1_L, W SUBWF AUX, W BTFSC STATUS, C GOTO GREU8_CLEAR_W BTFSC STATUS, Z GOTO GREU8_CLEAR_W MOVLW 0X01 MOVWF AUX RETLW 0 GREU8_CLEAR_W CLRF AUX RETLW 0 LESU8 BCF STATUS, RP0 MOVF AUX1_L, W SUBWF AUX, W BTFSS STATUS, C GOTO LESU8_CLEAR_W BTFSC STATUS, Z GOTO LESU8_CLEAR_W MOVLW 0X01 MOVWF AUX RETLW 0 LESU8_CLEAR_W CLRF AUX RETLW 0 EQU8 BCF STATUS, RP0 MOVF AUX1_L, W SUBWF AUX, W BTFSS STATUS, Z GOTO EQU8_CLEAR_W MOVLW 0X01 MOVWF AUX RETLW 0 EQU8_CLEAR_W CLRF AUX RETLW 0 PROGRAM_START __CONFIG H'3FFE';* ; ---------------------------------------------------------------------------------------------------- ; WRITTEN BY TED CALLAHAN 10-FEB-04 REV A ; THIS ROUTINE IMPLEMENTS A BASIC SKID-STEER MIXER WITH INPUT FROM A DIGITAL PROPORTIONAL RC RECEIVER. ; TESTED WITH A FUTABA 7CH RECEIVER. INPUTS ARE RB0, RB1, RB2. RB2 IS AN AUX INPUT. INTERRUPT RB0 IS USED ; TO SENSE A FRAME. AS RB0 RISES, A PULSE COUNT OF EACH INPUT BEGINS. THE COUNT LOOP REQUIRES ; ~55US FOR EACH PASS AT 8MHZ CLOCK. INPUT OF 1MS PULSE=~18, 1.5MS=~27, 2MS=~36. THE ROUTINE SENSES ; STICK CENTER FOR EACH INPUT AT POWER UP SO ALL STICKS SHOULD BE NEUTRALIZED (TRIM CENTER) ; INPUTS: RB0(6)-XIN SERVO 1 (AILERON), RB1(7)-YIN SERVO 2 (ELEVATOR), RB2(8)-ZIN SERVO 3 (RUDDER) FROM RECEIVER ; OUTPUTS: RB3(9)-LF, RB4(10)-LR, RB5(11)-RF, RB6(12)-RR, RA0(17)-AUX RIGHT, RA1(18)-AUX LEFT ; RB7 (13) IS PULLED LOW TO LIGHT AN ON STATUS LED IF DESIRED. ; ---------------------------------------------------------------------------------------------------- MAIN BCF STATUS, RP0 ; SET THE WATCHDOG TIMER TO TIME OUT AFTER 576MS BSF STATUS, RP0;* MOVLW B'11011101';* MOVWF OPTION_REG;* CLRWDT;* MOVLW B'11011101';* MOVWF OPTION_REG;* BCF STATUS, RP0;* MOVLW B'10000' BSF STATUS, RP0 MOVWF TRISA MOVLW B'00000111' MOVWF TRISB BCF STATUS, RP0 CLRF XCNT CLRF YCNT CLRF ZCNT CLRF XIN CLRF YIN CLRF ZIN CLRF AUXUP CLRF AUXDN CLRF FSTART BCF PORTA, 0 BCF PORTA, 1 BCF PORTA, 2 BCF PORTB, 7 BCF PORTB, 6 BCF PORTB, 5 BCF PORTB, 4 BCF PORTB, 3 ; INIT IS A FLAG FOR GRABBING STICK CENTERS ON POWER UP MOVLW 1 MOVWF INIT BSF INTCON, GIE BSF STATUS, RP0 BSF OPTION_REG, INTEDG BSF INTCON, INTE INTWAIT BSF STATUS, RP0 ; WAIT FOR A FRAME START INTERRUPT (FSTART=1) BCF STATUS, RP0 MOVF FSTART, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVLW 1 XORWF AUX, F MOVF AUX, W BTFSS AUX, 0 GOTO ENDIFLAB1 GOTO INTWAIT ENDIFLAB1 BCF STATUS, RP0 ; IF NO INTERRUPT OCCURS WITHIN 576MS THE WATCHDOG TIMER WILL RESET AND TURN EVERYTHING OFF ; OTHERWISE, CLEAR THE TIMER AT THIS POINT CLRWDT;* ; A FRAME HAS NOW STARTED (FSTART=1) CNTLOOP BCF STATUS, RP0 MOVLW 0 BTFSS PORTB, 0 GOTO IOLAB2 MOVLW 1 IOLAB2 BCF STATUS, RP0 MOVWF XIN MOVLW 0 BTFSS PORTB, 1 GOTO IOLAB3 MOVLW 1 IOLAB3 BCF STATUS, RP0 MOVWF YIN MOVLW 0 BTFSS PORTB, 2 GOTO IOLAB4 MOVLW 1 IOLAB4 BCF STATUS, RP0 MOVWF ZIN MOVF XIN, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W BTFSS AUX, 0 GOTO ENDIFLAB5 INCF XCNT, F ENDIFLAB5 BCF STATUS, RP0 MOVF YIN, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W BTFSS AUX, 0 GOTO ENDIFLAB6 INCF YCNT, F ENDIFLAB6 BCF STATUS, RP0 MOVF ZIN, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W BTFSS AUX, 0 GOTO ENDIFLAB7 INCF ZCNT, F ENDIFLAB7 BCF STATUS, RP0 MOVF FSTART, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W BTFSS AUX, 0 GOTO ENDIFLAB8 GOTO CNTLOOP ENDIFLAB8 BCF STATUS, RP0 ; FRAME HAS ENDED (FSTART=0) ; IF INIT=1 THEN STORE THE CENTER POSITIONS AND RECOUNT MOVF INIT, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W BTFSS AUX, 0 GOTO ENDIFLAB9 ; IF THE CENTERS ARE WAY OUT OF NOMINAL (~1.5MS OR ~27) THEN ADJUST TO NOMINAL 1.5MS MOVF XCNT, W MOVWF S1 MOVLW 24 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF XCNT, W MOVWF S2 MOVLW 30 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB10 MOVF XCNT, W MOVWF XCENT GOTO ELSELAB10 ENDIFLAB10 BCF STATUS, RP0 MOVLW 27 MOVWF XCENT ELSELAB10 BCF STATUS, RP0 MOVF YCNT, W MOVWF S1 MOVLW 24 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YCNT, W MOVWF S2 MOVLW 30 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB11 MOVF YCNT, W MOVWF YCENT GOTO ELSELAB11 ENDIFLAB11 BCF STATUS, RP0 MOVLW 27 MOVWF YCENT ELSELAB11 BCF STATUS, RP0 MOVF ZCNT, W MOVWF S1 MOVLW 24 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF ZCNT, W MOVWF S2 MOVLW 30 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB12 MOVF ZCNT, W MOVWF ZCENT GOTO ELSELAB12 ENDIFLAB12 BCF STATUS, RP0 MOVLW 27 MOVWF ZCENT ELSELAB12 BCF STATUS, RP0 CLRF INIT CLRF XCNT CLRF YCNT CLRF ZCNT GOTO INTWAIT ENDIFLAB9 BCF STATUS, RP0 ; TURN OFF THE INTERRUPT TO ALLOW TIME TO PROCESS THE COUNTS BCF INTCON, GIE ; PROCESS THE COUNTS FROM HERE CLRF YHI CLRF YLO CLRF XHI CLRF XLO CLRF YOFF CLRF XOFF ; DECODE THE STICK POSITIONS BASED ON THE COUNTS MOVF YCENT, W MOVWF S1 MOVLW 22 ADDWF S1, W MOVWF S1 MOVF YCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YCNT, W MOVWF S2 MOVF YCENT, W MOVWF S3 MOVLW 4 ADDWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB13 MOVLW 1 MOVWF YLO ENDIFLAB13 BCF STATUS, RP0 MOVF YCENT, W MOVWF S1 MOVLW 4 ADDWF S1, W MOVWF S1 MOVF YCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVLW 1 XORWF AUX, F MOVF AUX, W MOVWF S1 MOVF YCNT, W MOVWF S2 MOVF YCENT, W MOVWF S3 MOVLW 4 SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVLW 1 XORWF AUX, F MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB14 MOVLW 1 MOVWF YOFF ENDIFLAB14 BCF STATUS, RP0 MOVF YCENT, W MOVWF S1 MOVLW 22 SUBWF S1, W MOVWF S1 MOVF YCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YCNT, W MOVWF S2 MOVF YCENT, W MOVWF S3 MOVLW 4 SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB15 MOVLW 1 MOVWF YHI ENDIFLAB15 BCF STATUS, RP0 MOVF XCENT, W MOVWF S1 MOVLW 22 ADDWF S1, W MOVWF S1 MOVF XCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF XCNT, W MOVWF S2 MOVF XCENT, W MOVWF S3 MOVLW 4 ADDWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB16 MOVLW 1 MOVWF XHI ENDIFLAB16 BCF STATUS, RP0 MOVF XCENT, W MOVWF S1 MOVLW 4 ADDWF S1, W MOVWF S1 MOVF XCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVLW 1 XORWF AUX, F MOVF AUX, W MOVWF S1 MOVF XCNT, W MOVWF S2 MOVF XCENT, W MOVWF S3 MOVLW 4 SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVLW 1 XORWF AUX, F MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB17 MOVLW 1 MOVWF XOFF ENDIFLAB17 BCF STATUS, RP0 MOVF XCENT, W MOVWF S1 MOVLW 22 SUBWF S1, W MOVWF S1 MOVF XCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF XCNT, W MOVWF S2 MOVF XCENT, W MOVWF S3 MOVLW 4 SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB18 MOVLW 1 MOVWF XLO ENDIFLAB18 BCF STATUS, RP0 ; DO THE Z AXIS DECODING AND PIN SWITCHING ALL AT ONCE MOVF ZCENT, W MOVWF S1 MOVLW 22 ADDWF S1, W MOVWF S1 MOVF ZCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF ZCNT, W MOVWF S2 MOVF ZCENT, W MOVWF S3 MOVLW 4 ADDWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB19 BSF PORTA, 0 ENDIFLAB19 BCF STATUS, RP0 MOVF ZCENT, W MOVWF S1 MOVLW 4 ADDWF S1, W MOVWF S1 MOVF ZCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF ZCNT, W MOVWF S2 MOVF ZCENT, W MOVWF S3 MOVLW 4 SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB20 BCF PORTA, 0 BCF PORTA, 1 ENDIFLAB20 BCF STATUS, RP0 MOVF ZCENT, W MOVWF S1 MOVLW 22 SUBWF S1, W MOVWF S1 MOVF ZCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF ZCNT, W MOVWF S2 MOVF ZCENT, W MOVWF S3 MOVLW 4 SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB21 BSF PORTA, 1 ENDIFLAB21 BCF STATUS, RP0 MOVF ZCNT, W MOVWF S1 MOVLW 5 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF ZCNT, W MOVWF S2 MOVLW 40 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W IORWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB22 ; NO Z SIGNAL RECEIVED BCF PORTA, 0 BCF PORTA, 1 ENDIFLAB22 BCF STATUS, RP0 ; NOW SWITCH THE X&Y OUTPUT PINS ON AND OFF BASED ON THE STICK DECODING MOVF XCNT, W MOVWF S1 MOVLW 5 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YCNT, W MOVWF S2 MOVLW 5 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W IORWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB23 ; NO SIGNAL HAS BEEN RECEIVED FROM ONE OR BOTH CHANNELS SO TURN EVERYTHING OFF BCF PORTB, 3 BCF PORTB, 4 BCF PORTB, 5 BCF PORTB, 6 GOTO RECYCLE ENDIFLAB23 BCF STATUS, RP0 MOVF XOFF, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YOFF, W MOVWF S2 MOVLW 1 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB24 ; STOP BCF PORTB, 3 BCF PORTB, 4 BCF PORTB, 5 BCF PORTB, 6 ENDIFLAB24 BCF STATUS, RP0 MOVF YHI, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF XOFF, W MOVWF S2 MOVLW 1 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB25 ; FORWARD BCF PORTB, 4 BCF PORTB, 6 BSF PORTB, 5 BSF PORTB, 3 ENDIFLAB25 BCF STATUS, RP0 MOVF YLO, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF XOFF, W MOVWF S2 MOVLW 1 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB26 ; BACKWARD BCF PORTB, 3 BCF PORTB, 5 BSF PORTB, 4 BSF PORTB, 6 ENDIFLAB26 BCF STATUS, RP0 MOVF XHI, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YOFF, W MOVWF S2 MOVLW 1 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB27 ; RIGHT SPIN BCF PORTB, 4 BCF PORTB, 5 BSF PORTB, 3 BSF PORTB, 6 ENDIFLAB27 BCF STATUS, RP0 MOVF XLO, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YOFF, W MOVWF S2 MOVLW 1 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB28 ; LEFT SPIN BCF PORTB, 3 BCF PORTB, 6 BSF PORTB, 4 BSF PORTB, 5 ENDIFLAB28 BCF STATUS, RP0 MOVF XLO, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YHI, W MOVWF S2 MOVLW 1 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB29 ; LEFT FORWARD BCF PORTB, 3 BCF PORTB, 4 BCF PORTB, 6 BSF PORTB, 5 ENDIFLAB29 BCF STATUS, RP0 MOVF YHI, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF XHI, W MOVWF S2 MOVLW 1 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB30 ; RIGHT FORWARD BCF PORTB, 4 BCF PORTB, 5 BCF PORTB, 6 BSF PORTB, 3 ENDIFLAB30 BCF STATUS, RP0 MOVF XLO, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YLO, W MOVWF S2 MOVLW 1 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB31 ; LEFT BACKWARD BCF PORTB, 3 BCF PORTB, 4 BCF PORTB, 5 BSF PORTB, 6 ENDIFLAB31 BCF STATUS, RP0 MOVF XHI, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YLO, W MOVWF S2 MOVLW 1 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB32 ; RIGHT BACKWARD BCF PORTB, 3 BCF PORTB, 5 BCF PORTB, 6 BSF PORTB, 4 ENDIFLAB32 BCF STATUS, RP0 RECYCLE BCF STATUS, RP0 ; ZERO COUNTERS, TURN THE INTERRUPT ON, AND GO BACK AND WAIT FOR THE START OF A NEW FRAME CLRF FSTART CLRF XCNT CLRF YCNT CLRF ZCNT CLRF XIN CLRF YIN CLRF ZIN ; START THE PROCESS OVER AGAIN BSF INTCON, GIE GOTO INTWAIT ; GOTO SKIP_IR_EXT_BLOCK EXT_INTERRUPT_ROUTINE BCF STATUS, RP0 ; FRAME START INTERRUPT ROUTINE MOVF FSTART, W MOVWF S1 MOVLW 0 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP0 MOVF AUX, W BTFSS AUX, 0 GOTO ENDIFLAB33 ; A FRAME HAS STARTED MOVLW 1 MOVWF FSTART GOTO ELSELAB33 ENDIFLAB33 BCF STATUS, RP0 CLRF FSTART ELSELAB33 BCF STATUS, RP0 BCF INTCON, INTF GOTO RESTORE_CONTEXT SKIP_IR_EXT_BLOCK BCF STATUS, RP0 END