Sirius microSystems logo   Address and telephone numbers

Servo Control Code

Servos use a motor to move a disk or control arm +/- 45 degrees from a centered position in response to an electrical pulse. The servo's position is proportional to the duration of the pulse, with a 1ms pulse corresponding to the endpoint of travel in one direction, a 2ms pulse corresponding to the endpoint of travel in the opposite direction, and a 1.5ms pulse corresponding to the center position. This simple servo subroutine accepts an input position value of between 0 and 255 and creates a corresponding pulse from 1 to 2 ms in duration. This servo control code was designed to run in a 4 MHz PIC.

;The trick here is the padded delay loop. The Delay code is made to
;be 4 instruction cycles long. At 4 MHz, one instruction cycle is 1 us,
;and 4 instruction cycles take 4 us to execute, so we can execute 250 
;4-cycle delay loops per ms.

;The first time round, we wait for just under 1 ms (by preloading Counter
;with 248) after activating the servo. Then we use the position value to 
;set our delay length (in 4 us increments). When the delay finishes, we
;deactivate the servo. The result is a 1-2 ms pulse corresponding to the
;position variable.

;Hardware Equates               

Counter         EQU     0Ch             ;Servo delay counter register
Position        EQU     0Dh             ;Servo Position register

Servo1          EQU     RB.0            ;Servo I/O port pin

Main            .
Servo           MOVLW   248             ;Load Counter with # of cycles for
                MOVWF   Counter         ;1 ms Servo Delay
                BSF     Servo1          ;Activate the Servo1 output
                CALL    Delay           ;Wait for 1ms to pass
                MOVF    Position,0      ;Load W with servo position
                                        ;00 & FF=extremes, 128=centre
                MOVWF   Counter         ;and store in Counter for next delay
                CALL    Delay           ;Keep servo active for position delay
                BCF     Servo1          ;Deactivate the Servo1 output
                .                       ;Do other processing here, but
                                        ;remember to update each servo
                                        ;approximately once every 20 ms

Delay           ;Servo time delay
                NOP                     ;Pad the loop with one cycle
                DECFSZ  Counter         ;Counter=Counter-1
                GOTO    Delay           ;If Counter is not 0, repeat 
                RETURN                  ;If 0, return

© 2002 Sirius microSystems