2-Ch Hardware Servo Driver

'***************************************************************************
'*  Name    : 2CH_Servo.pbp
'*  Author  : Darrel Taylor
'*  Date    : 12/13/2009
'*  Notes   : 18F's only
'***************************************************************************
@ __CONFIG _CONFIG1H, _OSC_HSPLL_1H
@ __CONFIG _CONFIG2L, _BOREN_OFF_2L
@ __CONFIG _CONFIG2H, _WDT_OFF_2H
@ __CONFIG _CONFIG4L, _LVP_OFF_4L & _XINST_OFF_4L  

CLEAR

DEFINE OSC 40  
DEFINE  INTHAND myint 

;----[User Selections]------------------------------------------------------    
MoveSpeed CON 3                      ' movement speed smaller = faster
PosRange  CON 900                    ' position is 0-900, 0.0-90.0 degrees
                                     '             450 is Center
CCP1pin   VAR PORTC.2                ' Specify the pin that CCP1 uses
CCP2pin   VAR PORTC.1                ' Specify the pin that CCP2 uses

;----[Variables and Aliases]------------------------------------------------    
Servo1    VAR WORD                   ' Position for Servo1
Servo2    VAR WORD                   ' Position for Servo2
DutyHold1 VAR WORD BANK0 SYSTEM      ' holds next dutycycle, synch with PWM 
DutyHold2 VAR WORD BANK0 SYSTEM      '
TempW     VAR WORD                   ' temporary variable
POS       VAR WORD                   ' loop counter variable

TMR1IE    VAR PIE1.0                 ' Timer1 Interrupt Enable
TMR1IF    VAR PIR1.0                 ' Timer1 Interrupt Flag
TMR1ON    VAR T1CON.0                ' Timer1 ON bit
GIE       VAR INTCON.7               ' Global Interrupt Enable
PEIE      VAR INTCON.6               ' Peripheral Interrupt Enable

;----[Initialization]-------------------------------------------------------
Init:
    T1CON   = %00100000              ' Timer1 off, Prescaler 1:4 (40mhz)
                                     '          1:2 (20mhz), 1:1 (10mhz)
    TMR1IE  = 1                      ' Enable TMR1 overflow interrupt 
    TMR1IF  = 0                      ' Clear  TMR1 interrupt flag
    PEIE    = 1                      ' Enable peripheral interrupts
    GIE     = 1                      ' Enable global interrupts
    CCP1CON = 9                      ' Low on match TMR1 with DUTY1
    CCP2CON = 9                      ' Low on match TMR1 with DUTY2
    OUTPUT CCP1pin                   ' Set CCP1 pin to output, starts High
    OUTPUT CCP2pin                   ' Set CCP1 pin to output, starts High

    Servo1 = PosRange/2              ' Start at Center Position
    Servo2 = PosRange/2
    GOSUB SetServos
    TMR1ON = 1                       ' turn ON TMR1
    PAUSE 2000                       ' allow time for servo to Home


;----[Main Program Loop]----------------------------------------------------
Main:
    FOR POS = 0 TO PosRange          ' move both from one end to the other
        Servo1 = POS
        GOSUB SetServo1              ' Set servo1's position
        Servo2 = POS
        GOSUB SetServo2              ' Set servo2's position
        PAUSE MoveSpeed              ' movement speed
    NEXT POS

    FOR POS = PosRange TO 0 STEP -1  ' move servo1 back to home
        Servo1 = POS
        GOSUB SetServo1
        PAUSE MoveSpeed
    NEXT POS

    FOR POS = PosRange TO 0 STEP -1  ' move servo2 back to home
        Servo2 = POS
        GOSUB SetServo2
        PAUSE MoveSpeed
    NEXT POS

    PAUSE 1000
Goto Main                            ' rinse and repeat

;----[Convert Position in degrees to dutycycle]-----------------------------
SetServos:                           ' Set both Servo's dutycycles
    GOSUB SetServo1

SetServo2:                           ' scale Posistion to 2500 Dutycycle
    TempW = Servo2 * 2500
    TempW = DIV32 PosRange + 2500
    GIE = 0                          ' no ints during variable update
    DutyHold2 = TempW
    GIE = 1
RETURN

SetServo1:
    TempW = Servo1 * 2500            
    TempW = DIV32 PosRange + 2500
    GIE = 0
    DutyHold1 = TempW
    GIE = 1
RETURN

;----[Dual Servo Driver - Interrupt Service]--------------------------------
Asm
myint
    movlw     9                  ; %00001001 compare mode, low on match
    clrf      CCP1CON            ; clrf added per Bruce's suggestion
    movwf     CCP1CON            ; Set Pins to default state (High)
    clrf      CCP2CON            ; clrf added per Bruce's suggestion
    movwf     CCP2CON
    movf      DutyHold1+1,W      ; update current position's dutycycle
    movwf     CCPR1H             ; for both servos
    movf      DutyHold1,W
    movwf     CCPR1L
    movf      DutyHold2+1,W
    movwf     CCPR2H 
    movf      DutyHold2,W
    movwf     CCPR2L
    bcf       PIR1,0             ; Clear Timer Int Flag
    retfie    FAST               ; Return from interrupt with shadow regs
EndAsm