🚀 go-pugleaf

   Please, don't anybody hit me for wasting bandwidth with this LONG
posting!  There have been so many people requesting info about using
these little servo motors that I thought it would be ok to post the
driver.  Also, check the comment section at the end of the code re:
why I used a table driven approach vs calculated delays.  Hope this
will help those interested in tinkering with servos get started.
If you use this code as a starting point, how 'bout some feedback?

      (And PL-Ease, no more requests to translate this to C - I
      don't use C, I don't like C, an' I ain't gonna do it!  OK?)



        page    ,132
;------------------------------------------------------------------------
;
;  SERVO2C.ASM  - (C) R. LAMB 1988
;  Jan 7, 1989
;  Posting this for all those who asked how it was done.
;  This program will drive a servo in a demo pattern.
;  No wild claims are made about the coding style except that it works.
;------------------------------------------------------------------------
;OVERVIEW:
;       This program sends a Pulse Width Modulated pulse train
;       suitable for Radio Control type miniature servos.
;       LPT1: is used as the interface port.  It is suggested
;       that a buffer circuit be added to the printer port to
;       help protect the computer from dire disasters.
;       A small regulated +5 volt power supply is also recommended
;       to prevent motor EMF from feeding back into the computer.
;
;       For what it's worth, I didn't use a scope or even a calculator
;       to develop this - just a bit of "FEEL" for what I was after.
;	I simply started shipping different width pulses to the servo.
;       Once the servo started responding, the delay loops were tweaked
;       to locate the "ENDS" of the pulse widths for full travel.
;       The difference between full left and full right were divided
;       into 15 equal pieces - and it was done.
;
;       I have noticed that a 4.77 MHz PC tends to "jitter" the servo
;       a bit.  We spent some effort trying to track down the cause and
;       decided it was probably due to dRAM refresh (?).  Curiouser...
;
;	The code uses software delay loops to produce the pulses.
;	OK, it ain't "elegant", but it is cheap and simple.  If you
;	play aroung with it on a faster machine, you will have to 
;	tweek the loops to match.  On the origional (a 6 mhz Z80),
;	the delays were managed by the task supervisor, which used
;	a counter-timer for the delay tics.  True multi-tasking stuff.
;
;       For an 8 Mhz XT, the delay loops look something like this:
;
;         PW = 160      = pulse width
;         first "half" of pulse = 1 Msec lead-in
;               mov ch,0	
;               mov cl,bl
;               2 nop's
;               loop
;         second "half" of pulse = PWM position = 0 to 1 Msec
;               mov ch,0
;               mov cl,bl
;               push ax
;               pop  ax
;               2 nop's
;               loop
;
;-----------------------------------------------------------
;
;       Minimal Interface for Printer Port:
;       One 7404 Hex inverter can provide 2 servo channels.
;
;                                   Regulator
;                                    _______
;         9 v DC in                 |       | 5 v DC out
;       >-------------------*-------| 7805  |--------*---, 
;                           |       |_______|        |  ===
;                          ===         |             |   |
;                           |          |             |   V    
;                           V          |             |
;                                      |      ,------*---<] Motor +5 volts
;       Printer bit   1|\ 2    3|\ 4   |      |
;    2  >--------------| >o--*--| >o---------.|.---------<] Servo PWM Signal
;       Printer ground |/    |  |/     |      |
;    25 >-------------------.|.--------*-----.|.---------<] Ground
;                            |         |      |            Wire this connector
;                            |        ___     |            to match your servo.
;                            |         -     ,-,
;                            |               |R|
;                            |               |_| 320 ohms
;                            |11|\ 12    //   |
;                            `--| >o----|>|---'
;                               |/
;                                       LED
;	The regulator is fed from a AC adapter, but a diode bridge and
;	transformer can also be used.  
;	The LED shows activity, and suprisingly, also shows the PULSE WIDTH!
;
;-----------------------------------------------------------
;EQUATES:
;
cr	equ	10h		; carrage return
lf	equ	13h		; line feed
PW      EQU     160             ; PULSE WIDTH DELAY
PC      EQU     100             ; PULSE TRAIN COUNT
PD      EQU     512		; DELAY BETWEEN PULSES
LPT1    EQU     0278H           ; PRINTER PORT 1 I/O ADDRESS
LPT2    EQU     0378H		; PRINTER PORT 2 I/O ADDRESS
LPT     EQU     LPT2            ; LPTn I/O ADDRESS (pick one)
wait_1  EQU     1000h           ; lunch duration (between pulse trains)
;----------------------------------------------------------------
cseg	segment 'code'
	assume	cs:cseg, ds:cseg, es:cseg
	org	100h		; for .COM file
servo	proc	far
	jmp	start

;----------------------------------------------------------------
;LOCAL STORAGE: 
CMD     DB       000H           ; where we want servo to go
SRV     DB       001H           ; servo channel number
POS     DB       0FFH           ; attempted position

Hello	db	'Servo demo 22C: $'  
p_mes   db      13,10,'Position: $'
c_mes   db      ' Code: $'
;----------------------------------------------------------------
start:	
	lea	dx, Hello
	call	print_msg
	call	newlin
	mov	dx, lpt		;get address of the port 
	mov	al, 0
        out     dx, al          ;set all bits low to start
 
s1:     mov     pos,0           ; init to the "HOME" position
 
feed:	mov	ah, 0bh		; see if it's time to quit
        int     21h             ; any key will stop us
	cmp	al, 0ffh	; key pressed
	jnz	go
 
        mov     al, 0           ; all bits low when quitting
	out	dx, al
	int	20h		; return to DOS
                                ;----------------
go:     inc     pos		; step to next position in the table
        cmp     pos, 0fh	; last pos done?
        jg      s1              ; if yes, wrap back to table pos 0
        lea     dx, p_mes	; show position message
        call    print_msg	;
 
        mov     al,pos          ; save next command position
        mov     cmd, al         ; where we want it to go
        mov     ah,0
        call    hex2con  	; show where we's at
        lea     dx, c_mes       ; say something nice
        call    print_msg
;                               ;----------------------
        mov     srv, 01         ; channel 1
        call    train           ; make a pulse train happen
        mov     ax, wait_1      ; wait loop iterations
        call    l_dlay          ; take a short coffee break
        mov     cmd, 0          ; move it back to HOME pos
        call    train           ; catch a train for the coast
        mov     al, pos         ; recover current pos
        mov     cmd,al          ; restore next command position
        mov     cx, 1           ;
HOLD:   PUSH    CX              ; delay between position cycles
        mov     ah, 25          ;  wait some
        call    l_dlay          ;   more
        POP     CX              ;    and
        LOOP    HOLD            ;     more
                                ;
        jmp     feed            ;
;                               ;-----------------------
servo	endp
;----------------------------------------------------------------
TRAIN	proc	near
 
;       CMD is table entry for positioning servo        AL
;	SRV is servo channel number			AH
;	BL is servo position code (after XLAT)
;	BH is the number of pulses to make		PC
 
	LEA	bx, srvtab	; POINT TO POSITION TABLE
	mov	al, CMD		; servo channel number
	XLAT			; XLAT POSITION CODE TO AL
 
        push    ax
        call    hex2con		; show n tell
        pop     ax
 
        MOV     BL, AL          ; MOVE POSITION CODE IN BL
	MOV	BH, PC		; NUMBER OF PULSES TO MAKE 
 
T_2:    PUSH    BX              ; SAVE POSITION/COUNT
	CALL	PULSE		;   MAKE A BIG ONE
 
        POP     BX              ; GRAB OUR GOODIES BACK
        DEC     BH              ; COUNT THAT PULSE OFF (COUNT DOWN)
	CMP	BH,0		; ALL PULSES SENT ?
	JNZ	T_2		; BIF NOT YET ...
 
        RET                     ; BACK TO CALLER !!!
;
PULSE:
        MOV     DX, LPT         ; ACTIVE PRINTER PORT
        MOV     AL, SRV         ; SELECT SERVO CHANNEL NUMBER
				; if doing several channels at once, you
				; need to preserve the current context
				; of the output port.  (74LS259's are BEST!)
;-------------------------------,
        OUT     DX, AL          ;     START OF THE PWM SIGNAL TO THE SERVO
PF_1:   mov     ch,0
        mov     cl,pw           ;     1st millisec half
PF_2:   nop                     ;
        nop                     ;
        loop    pf_2            ;
P_1:
        mov     ch,0            ;     2nd half
        mov     cl,bl           ;     BL HAS POSITION CODE FROM TABLE
P_2:    nop                     ;
        nop                     ;     SITTIN' ON THE DOCK
        push    ax              ;       OF THE BAY
        pop     ax              ;         WASTIN' TI-III-IME
        loop    p_2             ;
P_3:
        MOV     AL,0            ;     TURN PWM BITS OFF
        OUT     DX, AL          ; END OF THIS PULSE
;-------------------------------'
        MOV     AL, 05          ; OUTTER LOOP DELAY BETWEEN PULSES
PT_1:   MOV     CX, PD          ; INNER LOOP (PERIOD) DELAY VALUE
PT_2:
        LOOP    PT_2            ; INNER LOOP ITSELF
        DEC     AL              ; COUNT OUTTER LOOP
        CMP     AL, 0           ; ARE WE DONE YET ?
        JNZ     PT_1            ; BIF NOT YET ...

        RET                     ; ROLL ANOTHER ONE ...
TRAIN	endp
;----------------------------------------------------------------
L_DLAY  PROC    NEAR            ; LONG DELAY - Ax has tick iterations
	push	CX
	push	ax
LD_1:	MOV	CX, 0FH		; LONG 'NUF ?
LD_2:	LOOP	LD_2
 
	DEC	Ax
	CMP	Ax, 0
	JNZ	LD_1
 
        pop     ax
	POP	CX
	RET
L_DLAY	ENDP
;----------------------------------------------------------------
HEX2CON         PROC    NEAR
        PUSH    AX                      ;Save reg for second digit
        PUSH    CX                      ;Save CX for use in shift
        MOV     CL,4                    ;Shift 4 bits
        SHR     AL,CL                   ;Get high 4 bits in low end
        POP     CX                      ;Restore register
        CALL    H2C                     ;Print the digit in AL
        POP     AX                      ;Get back original AL
        AND     AL,0FH                  ;Take second digit
 
H2C:    ADD     AL,90H                  ;Convert AL to ASCII
        DAA
        ADC     AL,40H
        DAA
        MOV     AH,0EH                  ;Write TTY
        INT     10H                     ; Thru BIOS
        RET                             ;Double duty
HEX2CON ENDP
;----------------------------------------------------------------
newlin  proc    near
        push    ax
        push    dx
        mov     ah, 02h
        mov     dl, 13
        int     21h
        mov     ah, 02h
        mov     dl, 10
        int     21h
        pop     dx
        pop     ax
        ret
newlin  endp
;
PRINT_MSG	PROC	NEAR
	mov	ah, 9
	int	21h
        RET
PRINT_MSG	ENDP
 
;----------------------------------------------------------------
 
SRVTAB:
;       THIS PROGRAM USES A POSITION TABLE TO DETERMINE THE SERVO
;       PULSE WIDTH.  FOR THIS SIMPLE DEMO, IT MAY NOT BE APPARENT
;       WHY I WANTED THIS, BUT CONSIDER:
;       WHAT IF YOU NEEDED A NON-LINEAR FUNCTION?  SUPPOSE YOU WANTED
;       A _LOGRYTHMIC_ RESPONSE? OR SOME FUNKY ARBITRARY KINDA THING TO
;	TURN THE TUNING KNOB ON YOUR RADIO (hey, I don't know what you want!).
;       BY PLUGGING DELAY VALUES INTO THE TABLE THAT ARE NOT STRICTLY
;       LINEAR, YOU CAN CUSTOM TAILOR THE SERVO RESPONSE CURVE EASILY.
;       _AND_ NO MESSY FLOATING POINT CALCULATIONS TO FUSS WITH!
;
HOME:   DB       077H   ; HOME = POS 0  = CENTERED - FOR NOW ...
;
        DB       010H   ; RIGHT = 1 = CW
        DB       020H   ; 2
        DB       030H   ; 3
        DB       040H   ; 4
        DB       050H   ; 5
        DB       060H   ; 6
        DB       070H   ; 7
        DB       077H   ; CENTERED = 8
        DB       080H   ; 9
        DB       090H   ; 10
        DB       0A0H   ; 11
        DB       0B0H   ; 12
        DB       0C0H   ; 13
        DB       0D0H   ; 14
        db       0E0h   ; LEFT = 15 = CCW       ;
;------------------------------------------------------------------------
cseg	ends
;
	end	servo