lamb@brahms.udel.edu (Richard E Lamb) (02/20/91)
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 servolamb@brahms.udel.edu (Richard E Lamb) (02/20/91)
oops, Sorry, Chris. Thanks for the reminder: to Assemble the Servo driver: MASM SERVO; LINK SERVO; EXE2BIN SERVO.EXE SERVO.COM DEL SERVO.EXE
lamb@brahms.udel.edu (Richard E Lamb) (02/23/91)
In article <3417@polari.UUCP> mzenier@polari.UUCP (Mark Zenier) writes: >In article <18861@brahms.udel.edu> lamb@brahms.udel.edu (Richard E Lamb) writes: > >Isn't there an 18 Hz real time clock interrupt that would stick its >nose into any timing loops on a PC? > Yes, there is, but it tends to be a fairly constant time period unless there is a key entered. The jitter problem was real strange. It wasn't noticible at 8 Mhz, and I NEVER saw it on the Z-80 version. I admit that blaming it on refresh was perhaps grabbing at straws. It was never a really obnoxious problem, but I did spend an inordinant amount of time trying to figure it out..... any OTHER ideas?