MEL PICBASIC Forum - 2-CH Hardware Servo Driver


  • 2-CH Hardware Servo Driver

    Here's a 2 channel Hardware Servo Driver, that uses Timer1 and both CCP modules in Compare mode.
    With an 18F.

    Servo outputs are the CCP? pins.
    Pulses will be continuous at ~40hz, regardless of MainLoop activity.

    Cheers ...
    Code:
    '***************************************************************************
    '*  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
    This article was originally published in forum thread: 16 bit PWM using CCP1 started by Normnet View original post