PIC 16F628
FREQ 8
OSCILLATOR CRYSTAL
BYTE xin,yin,zin,fstart,init,AUXup,AUXdn,yhi,ylo,xhi
BYTE xlo,yoff,xoff,xtop,xbot,ytop,ybot,ztop,zbot,xcnt
BYTE ycnt,zcnt,xcent,ycent,zcent,crange
     *__CONFIG H'3F2E'
     '----------------------------------------------------------------------------------------------------
     ' Written by Ted Callahan   23-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=200.  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 :
     '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
     SetPort A, %10000
     SetPort B, %00000111
     ' crange is +/- range of stick center
     crange=10
     xtop=0
     xbot=0
     ytop=0
     ybot=0
     ztop=0
     zbot=0
     xcnt=0
     ycnt=0
     zcnt=0
     xin=0
     yin=0
     zin=0
     AUXup=0
     AUXdn=0
     fstart=0
     PinLow A, 0
     PinLow A, 1
     PinLow A, 2
     PinLow B, 7
     PinLow B, 6
     PinLow B, 5
     PinLow B, 4
     PinLow B, 3
     'init is a flag for grabbing stick centers on power up
     init=1
     Interrupt_Enable
     Interrupt_Ext 1, rising
intwait :
     'wait for a frame start interrupt (fstart=1)
     IF fstart<>1 THEN
        GOTO intwait
     END IF
     'if no interrupt occurs within 72ms 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 :
     *  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
     IF init=1 THEN
        'if the centers are way out of nominal (~1.5ms or ~150) then adjust to nominal 1.5ms
        IF xcnt>150-crange & xcnt<150+crange THEN
           xcent=xcnt
        ELSE
           xcent=150
        END IF
        IF ycnt>150-crange & ycnt<150+crange THEN
           ycent=ycnt
        ELSE
           ycent=150
        END IF
        IF zcnt>150-crange & zcnt<150+crange THEN
           zcent=zcnt
        ELSE
           zcent=150
        END IF
        init=0
        xcnt=0
        ycnt=0
        zcnt=0
        GOTO intwait
     END IF
     'turn off the interrupt to allow time to process the counts
     Interrupt_Disable
     'process the counts from here
     yhi=0
     ylo=0
     xhi=0
     xlo=0
     yoff=0
     xoff=0
     'decode the stick positions based on the counts
     IF ycent+60>ycnt & ycnt>ycent+crange THEN
        ylo=1
     END IF
     IF ycent+crange>=ycnt & ycnt>=ycent-crange THEN
        yoff=1
     END IF
     IF ycent-60<ycnt & ycnt<ycent-crange THEN
        yhi=1
     END IF
     IF xcent+60>xcnt & xcnt>xcent+crange THEN
        xhi=1
     END IF
     IF xcent+crange>=xcnt & xcnt>=xcent-crange THEN
        xoff=1
     END IF
     IF xcent-60<xcnt & xcnt<xcent-crange THEN
        xlo=1
     END IF
     'do the z axis decoding and pin switching all at once
     IF zcent+60>zcnt & zcnt>zcent+crange THEN
        PinHigh A, 0
     END IF
     IF zcent+crange>zcnt&zcnt>zcent-crange THEN
        PinLow A, 0
        PinLow A, 1
     END IF
     IF zcent-60<zcnt & zcnt<zcent-crange THEN
        PinHigh A, 1
     END IF
     IF zcnt<80 | zcnt>220 THEN
        'no z signal received
        PinLow A, 0
        PinLow A, 1
     END IF
     'now switch the X&Y output pins on and off based on the stick decoding
     IF xcnt<80 | ycnt<80 THEN
        'no signal has been received from one or both channels so turn everything off 
        GOSUB overfl
        GOTO recycle
     END IF
     IF xoff=1 & yoff=1 THEN
        'STOP 
        PinLow B, 3
        PinLow B, 4
        PinLow B, 5
        PinLow B, 6
     END IF
     IF yhi=1 & xoff=1 THEN
        'FORWARD
        PinLow B, 4
        PinLow B, 6
        PinHigh B, 5
        PinHigh B, 3
     END IF
     IF ylo=1 & xoff=1 THEN
        'BACKWARD
        PinLow B, 3
        PinLow B, 5
        PinHigh B, 4
        PinHigh B, 6
     END IF
     IF xhi=1 & yoff=1 THEN
        'RIGHT SPIN
        PinLow B, 4
        PinLow B, 5
        PinHigh B, 3
        PinHigh B, 6
     END IF
     IF xlo=1 & yoff=1 THEN
        'LEFT SPIN
        PinLow B, 3
        PinLow B, 6
        PinHigh B, 4
        PinHigh B, 5
     END IF
     IF xlo=1 & yhi=1 THEN
        'LEFT FORWARD
        PinLow B, 3
        PinLow B, 4
        PinLow B, 6
        PinHigh B, 5
     END IF
     IF yhi=1 & xhi=1 THEN
        'RIGHT FORWARD
        PinLow B, 4
        PinLow B, 5
        PinLow B, 6
        PinHigh B, 3
     END IF
     IF xlo=1 & ylo=1 THEN
        'LEFT BACKWARD
        PinLow B, 3
        PinLow B, 4
        PinLow B, 5
        PinHigh B, 6
     END IF
     IF xhi=1 & ylo=1 THEN
        'RIGHT BACKWARD
        PinLow B, 3
        PinLow B, 5
        PinLow B, 6
        PinHigh B, 4
     END IF
recycle :
     'zero counters, turn the interrupt on, and go back and wait for the start of a new frame
     fstart=0
     xcnt=0
     ycnt=0
     zcnt=0      
     xin=0
     yin=0
     zin=0
     'start the process over again
     Interrupt_Enable
     GOTO intwait
overfl :
     PinLow B, 3
     PinLow B, 4
     PinLow B, 5
     PinLow B, 6
     RETURN
     '
     IR_Ext_Begin
        'frame start interrupt routine 
        IF fstart=0 THEN
           'a frame has started
           fstart=1
        ELSE
           fstart=0
        END IF
     IR_Ext_End