Path: utzoo!utgpu!news-server.csri.toronto.edu!cs.utexas.edu!swrinde!elroy.jpl.nasa.gov!ncar!gatech!udel!brahms.udel.edu!lamb From: lamb@brahms.udel.edu (Richard E Lamb) Newsgroups: sci.electronics Subject: Servo driver Message-ID: <18861@brahms.udel.edu> Date: 19 Feb 91 16:30:20 GMT Distribution: all Organization: University of Delaware Lines: 330 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 Brought to you by Super Global Mega Corp .com