PIC 16F84A
FREQ 4
OSCILLATOR CRYSTAL
BYTE in1,in2,in3,spos1,spos,top,bottom,tcnt,rate,i
BYTE rate2x,center,sdir1,sdir2,lt
     '
     ' 1/6th Scale RC Stuart Tank - Servo Interface Controller
     ' Written by Ted Callahan   Rev. B   2/7/04
     ' spos1: second servo position  (fire)
     ' spos:  first servo position (elevation)
     ' rate:  adjusts the rate servo 1 (elevation) moves
     ' RA0 is servo 1 UP, RA1 is servo 1 DOWN, RA2 is servo 2 (fire)
     ' RB0 is servo 1 output, RB1 is servo 2 output
     ' 250 loops = 2ms, 130 loops = 1ms, 190 loops = 1.5ms
     ' Program using config bits 3FFA
     ' Rev B version incorporates 3ms pseudo debounce check on input pins
     ' 
     center=190
     top=250
     bottom=120
     tcnt=0        
     spos=center
     lt=0
     SetPort A, %00111
     SetPort B, %01110000
     PinLow B, 1
     PinLow B, 0
     'read pins to determine servo rotate directions sdir1=elevation sdir2=fire
     PinRd B, 4, sdir1
     PinRd B, 5, sdir2
     'select slow or fast rate for the elevation servo
     PinRd B, 6, rate2x
     IF rate2x=0 THEN
        rate=1
     ELSE
        rate=3
     END IF
     IF sdir2=0 THEN
        spos1=top
     ELSE
        spos1=bottom
     END IF
main :
     'read the elevation input
     IF sdir1=0 THEN
        PinRd A, 0, in1
        PinRd a, 1, in2
     ELSE
        PinRd a, 0, in2
        PinRd a, 1, in1
     END IF
     'read the fire input
     PinRd A, 2, in3
     Delay 3, 0
     IF sdir1=0 & in1=1 THEN
        PinRd a, 0, in1
     END IF
     IF sdir1=0 & in2=1 THEN
        PinRd a, 1, in2
     END IF
     IF sdir1=1 & in2=1 THEN
        PinRd A, 0, in2
     END IF
     IF sdir1=1 & in1=1 THEN
        PinRd A, 1, in1
     END IF
     IF in1 & top>=spos THEN
        ' Increment servo 1 upward
        spos=spos+rate
     END IF
     IF in2 & bottom<=spos THEN
        ' Decrement servo 1 downward
        spos=spos-rate
     END IF
     IF in3 & tcnt=0 THEN
        ' Move servo 2 to fire 
        tcnt=20
        IF sdir2=0 THEN
           spos1=bottom
        ELSE
           spos1=top
        END IF
     END IF
     IF tcnt>0 THEN
        'count the loops for the fire servo to wait before cycling back...           
        tcnt=tcnt-1
     ELSE
        'return the fire servo to rest position
        IF sdir2=0 THEN
           spos1=top
        ELSE
           spos1=bottom
        END IF
     END IF
     '  Set RB0 high  
     PinHigh B, 0
     '  Keep the pulse high for the time specified by spos
     FOR i = 1 TO spos
     NEXT 
     '  Set RB0, servo1, back low
     PinLow B, 0
     '  Set RB1 high and keep it high for the time specified by spos1
     PinHigh B, 1
     FOR i = 1 TO spos1
     NEXT 
     '  Set RB1, servo 2, low
     PinLow B, 1
     '  Delay for the appropriate time period (no longer than 20ms) before starting over
     Delay 15, 1
     'flash the status LED
     IF lt=90 THEN
        lt=0
     ELSE
        lt=lt+1
     END IF
     IF lt>85 & lt<90 THEN
        PinLow B, 3
     ELSE
        PinHigh B, 3
     END IF
     GOTO main