2-CH Hardware Servo Driver
And since you made me think of it ...
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
Bookmarks