LIST P= 16F628 INCLUDE "P16F628.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, RP1 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 0X7F STATUS_TEMP EQU 0X20 PCLATH_TEMP EQU 0X21 FSR_TEMP EQU 0X22 AUX1_H EQU 0X23 AUX1_L EQU 0X24 AUX2_H EQU 0X25 AUX2_L EQU 0X26 AUX EQU 0X27 S0 EQU 0X28 S1 EQU 0X29 S2 EQU 0X2A S3 EQU 0X2B TEMP_FSR_TEMP EQU 0X2C TEMP_AUX1_H EQU 0X2D TEMP_AUX1_L EQU 0X2E TEMP_AUX2_H EQU 0X2F TEMP_AUX2_L EQU 0X30 TEMP_AUX EQU 0X31 TEMP_S0 EQU 0X32 TEMP_S1 EQU 0X33 TEMP_S2 EQU 0X34 TEMP_S3 EQU 0X35 XIN EQU 0X36 YIN EQU 0X37 ZIN EQU 0X38 FSTART EQU 0X39 INIT EQU 0X3A AUXUP EQU 0X3B AUXDN EQU 0X3C YHI EQU 0X3D YLO EQU 0X3E XHI EQU 0X3F XLO EQU 0X40 YOFF EQU 0X41 XOFF EQU 0X42 XTOP EQU 0X43 XBOT EQU 0X44 YTOP EQU 0X45 YBOT EQU 0X46 ZTOP EQU 0X47 ZBOT EQU 0X48 XCNT EQU 0X49 YCNT EQU 0X4A ZCNT EQU 0X4B XCENT EQU 0X4C YCENT EQU 0X4D ZCENT EQU 0X4E CRANGE EQU 0X4F GREU8 BCF STATUS, RP1 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, RP1 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, RP1 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'3F2E';* ; ---------------------------------------------------------------------------------------------------- ; 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 ; 10US FOR EACH PASS AT 8MHZ CLOCK. INPUT OF 1MS PULSE=100, 1.5MS=150, 2MS=100. 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(18)-LF, RB4(1)-LR, RB5(2)-RF, RB6(3)-RR, RA0(9)-AUX RIGHT, RA1(10)-AUX LEFT ; RA2 IS PULLED LOW TO LIGHT AN ON STATUS LED IF DESIRED. ; ---------------------------------------------------------------------------------------------------- MAIN BCF STATUS, RP1 BCF STATUS, RP0 ; SET THE WATCHDOG TIMER TO TIMEOUT AFTER 72MS BSF STATUS, RP0;* MOVLW B'11011010';* MOVWF OPTION_REG;* CLRWDT;* MOVLW B'11011010';* MOVWF OPTION_REG;* BCF STATUS, RP0;* MOVLW B'10000' BSF STATUS, RP0 MOVWF TRISA MOVLW B'00000111' MOVWF TRISB ; CRANGE IS +/- RANGE OF STICK CENTER MOVLW 10 BCF STATUS, RP0 MOVWF CRANGE CLRF XTOP CLRF XBOT CLRF YTOP CLRF YBOT CLRF ZTOP CLRF ZBOT 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 INTCON, PEIE BSF OPTION_REG, INTEDG BSF INTCON, INTE INTWAIT BCF STATUS, RP1 BCF STATUS, RP0 ; WAIT FOR A FRAME START INTERRUPT (FSTART=1) MOVF FSTART, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVLW 1 XORWF AUX, F MOVF AUX, W BTFSS AUX, 0 GOTO ENDIFLAB1 GOTO INTWAIT ENDIFLAB1 BCF STATUS, RP1 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, RP1 BCF STATUS, RP0 BTFSC PORTB, 0 ; READ THE THREE INPUT PINS AND INCREMENT THE COUNTERS IF HI.;* INCF XCNT;* BTFSC STATUS, Z;* GOTO OVERFL ; XCNT OVERFLOW SO START AGAIN;* BTFSC PORTB, 1;* INCF YCNT;* BTFSC STATUS, Z;* GOTO OVERFL ; YCNT OVERFLOW;* BTFSC PORTB, 2;* INCF ZCNT;* BTFSC STATUS, Z;* GOTO OVERFL ; ZCNT OVERFLOW;* NOP ; NO-OP PADDING TO MAKE 10US LOOP;* NOP;* NOP;* BTFSC FSTART, 0;* GOTO CNTLOOP;* ; 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, RP1 BCF STATUS, RP0 MOVF AUX, W BTFSS AUX, 0 GOTO ENDIFLAB2 ; IF THE CENTERS ARE WAY OUT OF NOMINAL (~1.5MS OR ~27) THEN ADJUST TO NOMINAL 1.5MS MOVF XCNT, W MOVWF S1 MOVLW 150 MOVWF S2 MOVF CRANGE, W SUBWF S2, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF XCNT, W MOVWF S2 MOVLW 150 MOVWF S3 MOVF CRANGE, W ADDWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB3 MOVF XCNT, W MOVWF XCENT GOTO ELSELAB3 ENDIFLAB3 BCF STATUS, RP1 BCF STATUS, RP0 MOVLW 150 MOVWF XCENT ELSELAB3 BCF STATUS, RP1 BCF STATUS, RP0 MOVF YCNT, W MOVWF S1 MOVLW 150 MOVWF S2 MOVF CRANGE, W SUBWF S2, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YCNT, W MOVWF S2 MOVLW 150 MOVWF S3 MOVF CRANGE, W ADDWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB4 MOVF YCNT, W MOVWF YCENT GOTO ELSELAB4 ENDIFLAB4 BCF STATUS, RP1 BCF STATUS, RP0 MOVLW 150 MOVWF YCENT ELSELAB4 BCF STATUS, RP1 BCF STATUS, RP0 MOVF ZCNT, W MOVWF S1 MOVLW 150 MOVWF S2 MOVF CRANGE, W SUBWF S2, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF ZCNT, W MOVWF S2 MOVLW 150 MOVWF S3 MOVF CRANGE, W ADDWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB5 MOVF ZCNT, W MOVWF ZCENT GOTO ELSELAB5 ENDIFLAB5 BCF STATUS, RP1 BCF STATUS, RP0 MOVLW 150 MOVWF ZCENT ELSELAB5 BCF STATUS, RP1 BCF STATUS, RP0 CLRF INIT CLRF XCNT CLRF YCNT CLRF ZCNT GOTO INTWAIT ENDIFLAB2 BCF STATUS, RP1 BCF STATUS, RP0 ; TURN OFF THE INTERRUPT TO ALLOW TIME TO PROCESS THE COUNTS BCF INTCON, GIE BCF INTCON, PEIE ; 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 60 ADDWF S1, W MOVWF S1 MOVF YCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YCNT, W MOVWF S2 MOVF YCENT, W MOVWF S3 MOVF CRANGE, W ADDWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB6 MOVLW 1 MOVWF YLO ENDIFLAB6 BCF STATUS, RP1 BCF STATUS, RP0 MOVF YCENT, W MOVWF S1 MOVF CRANGE, W ADDWF S1, W MOVWF S1 MOVF YCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVLW 1 XORWF AUX, F MOVF AUX, W MOVWF S1 MOVF YCNT, W MOVWF S2 MOVF YCENT, W MOVWF S3 MOVF CRANGE, W SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVLW 1 XORWF AUX, F MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB7 MOVLW 1 MOVWF YOFF ENDIFLAB7 BCF STATUS, RP1 BCF STATUS, RP0 MOVF YCENT, W MOVWF S1 MOVLW 60 SUBWF S1, W MOVWF S1 MOVF YCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YCNT, W MOVWF S2 MOVF YCENT, W MOVWF S3 MOVF CRANGE, W SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB8 MOVLW 1 MOVWF YHI ENDIFLAB8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF XCENT, W MOVWF S1 MOVLW 60 ADDWF S1, W MOVWF S1 MOVF XCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF XCNT, W MOVWF S2 MOVF XCENT, W MOVWF S3 MOVF CRANGE, W ADDWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB9 MOVLW 1 MOVWF XHI ENDIFLAB9 BCF STATUS, RP1 BCF STATUS, RP0 MOVF XCENT, W MOVWF S1 MOVF CRANGE, W ADDWF S1, W MOVWF S1 MOVF XCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVLW 1 XORWF AUX, F MOVF AUX, W MOVWF S1 MOVF XCNT, W MOVWF S2 MOVF XCENT, W MOVWF S3 MOVF CRANGE, W SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVLW 1 XORWF AUX, F MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB10 MOVLW 1 MOVWF XOFF ENDIFLAB10 BCF STATUS, RP1 BCF STATUS, RP0 MOVF XCENT, W MOVWF S1 MOVLW 60 SUBWF S1, W MOVWF S1 MOVF XCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF XCNT, W MOVWF S2 MOVF XCENT, W MOVWF S3 MOVF CRANGE, W SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB11 MOVLW 1 MOVWF XLO ENDIFLAB11 BCF STATUS, RP1 BCF STATUS, RP0 ; DO THE Z AXIS DECODING AND PIN SWITCHING ALL AT ONCE MOVF ZCENT, W MOVWF S1 MOVLW 60 ADDWF S1, W MOVWF S1 MOVF ZCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF ZCNT, W MOVWF S2 MOVF ZCENT, W MOVWF S3 MOVF CRANGE, W ADDWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB12 BSF PORTA, 0 ENDIFLAB12 BCF STATUS, RP1 BCF STATUS, RP0 MOVF ZCENT, W MOVWF S1 MOVF CRANGE, W ADDWF S1, W MOVWF S1 MOVF ZCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF ZCNT, W MOVWF S2 MOVF ZCENT, W MOVWF S3 MOVF CRANGE, W SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB13 BCF PORTA, 0 BCF PORTA, 1 ENDIFLAB13 BCF STATUS, RP1 BCF STATUS, RP0 MOVF ZCENT, W MOVWF S1 MOVLW 60 SUBWF S1, W MOVWF S1 MOVF ZCNT, W MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF ZCNT, W MOVWF S2 MOVF ZCENT, W MOVWF S3 MOVF CRANGE, W SUBWF S3, W MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB14 BSF PORTA, 1 ENDIFLAB14 BCF STATUS, RP1 BCF STATUS, RP0 MOVF ZCNT, W MOVWF S1 MOVLW 80 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF ZCNT, W MOVWF S2 MOVLW 220 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL GREU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W IORWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB15 ; NO Z SIGNAL RECEIVED BCF PORTA, 0 BCF PORTA, 1 ENDIFLAB15 BCF STATUS, RP1 BCF STATUS, RP0 ; NOW SWITCH THE X&Y OUTPUT PINS ON AND OFF BASED ON THE STICK DECODING MOVF XCNT, W MOVWF S1 MOVLW 80 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVWF S1 MOVF YCNT, W MOVWF S2 MOVLW 80 MOVWF AUX MOVF S2, W MOVWF AUX1_L CALL LESU8 BCF STATUS, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W IORWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB16 ; NO SIGNAL HAS BEEN RECEIVED FROM ONE OR BOTH CHANNELS SO TURN EVERYTHING OFF CALL OVERFL BCF STATUS, RP1 BCF STATUS, RP0 GOTO RECYCLE ENDIFLAB16 BCF STATUS, RP1 BCF STATUS, RP0 MOVF XOFF, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP1 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, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB17 ; STOP BCF PORTB, 3 BCF PORTB, 4 BCF PORTB, 5 BCF PORTB, 6 ENDIFLAB17 BCF STATUS, RP1 BCF STATUS, RP0 MOVF YHI, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP1 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, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB18 ; FORWARD BCF PORTB, 4 BCF PORTB, 6 BSF PORTB, 5 BSF PORTB, 3 ENDIFLAB18 BCF STATUS, RP1 BCF STATUS, RP0 MOVF YLO, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP1 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, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB19 ; BACKWARD BCF PORTB, 3 BCF PORTB, 5 BSF PORTB, 4 BSF PORTB, 6 ENDIFLAB19 BCF STATUS, RP1 BCF STATUS, RP0 MOVF XHI, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP1 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, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB20 ; RIGHT SPIN BCF PORTB, 4 BCF PORTB, 5 BSF PORTB, 3 BSF PORTB, 6 ENDIFLAB20 BCF STATUS, RP1 BCF STATUS, RP0 MOVF XLO, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP1 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, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB21 ; LEFT SPIN BCF PORTB, 3 BCF PORTB, 6 BSF PORTB, 4 BSF PORTB, 5 ENDIFLAB21 BCF STATUS, RP1 BCF STATUS, RP0 MOVF XLO, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP1 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, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB22 ; LEFT FORWARD BCF PORTB, 3 BCF PORTB, 4 BCF PORTB, 6 BSF PORTB, 5 ENDIFLAB22 BCF STATUS, RP1 BCF STATUS, RP0 MOVF YHI, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP1 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, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB23 ; RIGHT FORWARD BCF PORTB, 4 BCF PORTB, 5 BCF PORTB, 6 BSF PORTB, 3 ENDIFLAB23 BCF STATUS, RP1 BCF STATUS, RP0 MOVF XLO, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP1 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, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB24 ; LEFT BACKWARD BCF PORTB, 3 BCF PORTB, 4 BCF PORTB, 5 BSF PORTB, 6 ENDIFLAB24 BCF STATUS, RP1 BCF STATUS, RP0 MOVF XHI, W MOVWF S1 MOVLW 1 MOVWF AUX MOVF S1, W MOVWF AUX1_L CALL EQU8 BCF STATUS, RP1 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, RP1 BCF STATUS, RP0 MOVF AUX, W MOVF S1, W ANDWF AUX, F BTFSS AUX, 0 GOTO ENDIFLAB25 ; RIGHT BACKWARD BCF PORTB, 3 BCF PORTB, 5 BCF PORTB, 6 BSF PORTB, 4 ENDIFLAB25 BCF STATUS, RP1 BCF STATUS, RP0 RECYCLE BCF STATUS, RP1 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 BSF INTCON, PEIE GOTO INTWAIT OVERFL BCF STATUS, RP1 BCF STATUS, RP0 BCF PORTB, 3 BCF PORTB, 4 BCF PORTB, 5 BCF PORTB, 6 RETURN ; GOTO SKIP_IR_EXT_BLOCK EXT_INTERRUPT_ROUTINE BCF STATUS, RP1 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, RP1 BCF STATUS, RP0 MOVF AUX, W BTFSS AUX, 0 GOTO ENDIFLAB26 ; A FRAME HAS STARTED MOVLW 1 MOVWF FSTART GOTO ELSELAB26 ENDIFLAB26 BCF STATUS, RP1 BCF STATUS, RP0 CLRF FSTART ELSELAB26 BCF STATUS, RP1 BCF STATUS, RP0 BCF INTCON, INTF GOTO RESTORE_CONTEXT SKIP_IR_EXT_BLOCK BCF STATUS, RP1 BCF STATUS, RP0 END