;This 68HC11 program will use an OC5 interrupt to perform
;pulse width modulation in order to control a Hobby Servo Motor
tcnt    .cequ   $100E   ;address of the tcnt register
toc5    .cequ   $101E   ;address of TOC5 register
portb   .cequ   $1004   ;address of Port B
tflag1  .cequ   $1023
tmsk1   .cequ   $1022
        .org    $0000
        jmp     main    ;jump to main program segment
pos     .rs     1       ;User RAM variable to hold the desired
                        ;servo position [0 <= (pos) <= 100] where
                        ;0 = full CCW, 100 = full CW
save    .rs     2       ;temporary variable space in RAM
        .org    $0100
main:   jsr     init    ;initialize OC5 interrupts etc.
loop0:  clr     pos     ;clear position variable to 0
loop1:  inc     pos
        bsr     delay
        ldaa    #100
        cmpa    pos     ;if position variable = 100, reset it to zero
        beq     loop0
        bra     loop1   ;otherwise keep incrementing pos

delay:   ;this subroutine does a short delay
        ldx     #$2000  ;should delay for
loop2:  dex             ; about (3+3)x(2^13)x(500ns) = 0.025 sec
        bne     loop2
        rts

init:   ldaa    #$7E
        staa    $00D3   ;setup OC5 psuedo interrupt vector
        ldd     #myoc5
        std     $00D4
        clr     pos     ;initialize pos variable to zero(full CCW)
        ldx     #tmsk1  ;
        bset    1,X,$08 ;clear OC5 interrupt flag
        bset    0,X,$08 ;unmask local OC5 interrupt
        cli             ;unmask global interrupt to enable OC5
        rts

myoc5:  ;Interrupt handler for an OC5 interrupt/1st see if PB1 is on or off
mybit: .cequ    b'00000010     ;This determines which portB bit is used to drive the servo - change as desired
        ldx     #portb
on:     brclr   0,X,mybit,off      ;branch if bit currently off
        bclr    0,X,mybit           ;else turn if off
        ;now calculate how long to leave the bit turned off
        ;ton = (pos/100)*2000 + 2000 = 2000 + 20*pos
        ;toff = 20000 - ton = 18000 - 20*pos
        ldaa    pos
        ldab    #20
        mul             ;(A) x (B) --> (A)
        std     save    ;temporarily store result
        ldd     #18000
        subd    save    ;result now in  D - add result to current TCNT Contents
        addd    tcnt
        std     toc5    ;save result in oc5 register
        bra     done    ;reset interrupt flag and return from interrupt

off:    ldx     #portb  ;turn the bit on
        bset    0,X,mybit
        ldaa    pos
        ldab    #20
        mul
        addd    #2000
        addd    tcnt
        std     toc5
done:   ldx     #tflag1
        bset    0,X,$08  ;clear OC5 interrupt flag
        rti
        .end